From a8773fdafd2b2c3dab0deda37c2f0c3915873dfe Mon Sep 17 00:00:00 2001 From: "jack.song" Date: Thu, 2 Jan 2025 11:18:18 +0800 Subject: [PATCH 01/13] add interpolation Signed-off-by: jack.song Signed-off-by: NorahXiong --- common/autoware_interpolation/CHANGELOG.rst | 51 ++++ common/autoware_interpolation/CMakeLists.txt | 24 ++ common/autoware_interpolation/README.md | 109 +++++++ .../interpolation/interpolation_utils.hpp | 114 +++++++ .../interpolation/linear_interpolation.hpp | 35 +++ .../spherical_linear_interpolation.hpp | 48 +++ .../interpolation/spline_interpolation.hpp | 97 ++++++ .../spline_interpolation_points_2d.hpp | 89 ++++++ .../interpolation/zero_order_hold.hpp | 81 +++++ common/autoware_interpolation/package.xml | 24 ++ .../src/linear_interpolation.cpp | 59 ++++ .../src/spherical_linear_interpolation.cpp | 73 +++++ .../src/spline_interpolation.cpp | 247 ++++++++++++++++ .../src/spline_interpolation_points_2d.cpp | 212 +++++++++++++ .../test/src/test_interpolation.cpp | 21 ++ .../test/src/test_interpolation_utils.cpp | 138 +++++++++ .../test/src/test_linear_interpolation.cpp | 95 ++++++ .../test_spherical_linear_interpolation.cpp | 137 +++++++++ .../test/src/test_spline_interpolation.cpp | 279 ++++++++++++++++++ .../test_spline_interpolation_points_2d.cpp | 223 ++++++++++++++ .../test/src/test_zero_order_hold.cpp | 158 ++++++++++ 21 files changed, 2314 insertions(+) create mode 100644 common/autoware_interpolation/CHANGELOG.rst create mode 100644 common/autoware_interpolation/CMakeLists.txt create mode 100644 common/autoware_interpolation/README.md create mode 100644 common/autoware_interpolation/include/autoware/interpolation/interpolation_utils.hpp create mode 100644 common/autoware_interpolation/include/autoware/interpolation/linear_interpolation.hpp create mode 100644 common/autoware_interpolation/include/autoware/interpolation/spherical_linear_interpolation.hpp create mode 100644 common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp create mode 100644 common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp create mode 100644 common/autoware_interpolation/include/autoware/interpolation/zero_order_hold.hpp create mode 100644 common/autoware_interpolation/package.xml create mode 100644 common/autoware_interpolation/src/linear_interpolation.cpp create mode 100644 common/autoware_interpolation/src/spherical_linear_interpolation.cpp create mode 100644 common/autoware_interpolation/src/spline_interpolation.cpp create mode 100644 common/autoware_interpolation/src/spline_interpolation_points_2d.cpp create mode 100644 common/autoware_interpolation/test/src/test_interpolation.cpp create mode 100644 common/autoware_interpolation/test/src/test_interpolation_utils.cpp create mode 100644 common/autoware_interpolation/test/src/test_linear_interpolation.cpp create mode 100644 common/autoware_interpolation/test/src/test_spherical_linear_interpolation.cpp create mode 100644 common/autoware_interpolation/test/src/test_spline_interpolation.cpp create mode 100644 common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp create mode 100644 common/autoware_interpolation/test/src/test_zero_order_hold.cpp diff --git a/common/autoware_interpolation/CHANGELOG.rst b/common/autoware_interpolation/CHANGELOG.rst new file mode 100644 index 0000000000..37fc48c525 --- /dev/null +++ b/common/autoware_interpolation/CHANGELOG.rst @@ -0,0 +1,51 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_interpolation +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.40.0 (2024-12-12) +------------------- +* Merge branch 'main' into release-0.40.0 +* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" + This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. +* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) +* chore(package.xml): bump version to 0.39.0 (`#9587 `_) + * chore(package.xml): bump version to 0.39.0 + * fix: fix ticket links in CHANGELOG.rst + * fix: remove unnecessary diff + --------- + Co-authored-by: Yutaka Kondo +* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) +* fix(cpplint): include what you use - common (`#9564 `_) +* 0.39.0 +* update changelog +* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) +* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) +* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) + * unify package.xml version to 0.37.0 + * remove system_monitor/CHANGELOG.rst + * add changelog + * 0.38.0 + --------- +* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Yutaka Kondo + +0.39.0 (2024-11-25) +------------------- +* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) +* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) +* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) + * unify package.xml version to 0.37.0 + * remove system_monitor/CHANGELOG.rst + * add changelog + * 0.38.0 + --------- +* Contributors: Esteve Fernandez, Yutaka Kondo + +0.38.0 (2024-11-08) +------------------- +* unify package.xml version to 0.37.0 +* refactor(autoware_interpolation): prefix package and namespace with autoware (`#8088 `_) + Co-authored-by: kosuke55 +* Contributors: Esteve Fernandez, Yutaka Kondo + +0.26.0 (2024-04-03) +------------------- diff --git a/common/autoware_interpolation/CMakeLists.txt b/common/autoware_interpolation/CMakeLists.txt new file mode 100644 index 0000000000..09797272a2 --- /dev/null +++ b/common/autoware_interpolation/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_interpolation) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(autoware_interpolation SHARED + src/linear_interpolation.cpp + src/spline_interpolation.cpp + src/spline_interpolation_points_2d.cpp + src/spherical_linear_interpolation.cpp +) + +if(BUILD_TESTING) + file(GLOB_RECURSE test_files test/**/*.cpp) + + ament_add_ros_isolated_gtest(test_interpolation ${test_files}) + + target_link_libraries(test_interpolation + autoware_interpolation + ) +endif() + +ament_auto_package() diff --git a/common/autoware_interpolation/README.md b/common/autoware_interpolation/README.md new file mode 100644 index 0000000000..5801c06dcb --- /dev/null +++ b/common/autoware_interpolation/README.md @@ -0,0 +1,109 @@ +# Interpolation package + +This package supplies linear and spline interpolation functions. + +## Linear Interpolation + +`lerp(src_val, dst_val, ratio)` (for scalar interpolation) interpolates `src_val` and `dst_val` with `ratio`. +This will be replaced with `std::lerp(src_val, dst_val, ratio)` in `C++20`. + +`lerp(base_keys, base_values, query_keys)` (for vector interpolation) applies linear regression to each two continuous points whose x values are`base_keys` and whose y values are `base_values`. +Then it calculates interpolated values on y-axis for `query_keys` on x-axis. + +## Spline Interpolation + +`spline(base_keys, base_values, query_keys)` (for vector interpolation) applies spline regression to each two continuous points whose x values are`base_keys` and whose y values are `base_values`. +Then it calculates interpolated values on y-axis for `query_keys` on x-axis. + +### Evaluation of calculation cost + +We evaluated calculation cost of spline interpolation for 100 points, and adopted the best one which is tridiagonal matrix algorithm. +Methods except for tridiagonal matrix algorithm exists in `spline_interpolation` package, which has been removed from Autoware. + +| Method | Calculation time | +| --------------------------------- | ---------------- | +| Tridiagonal Matrix Algorithm | 0.007 [ms] | +| Preconditioned Conjugate Gradient | 0.024 [ms] | +| Successive Over-Relaxation | 0.074 [ms] | + +### Spline Interpolation Algorithm + +Assuming that the size of `base_keys` ($x_i$) and `base_values` ($y_i$) are $N + 1$, we aim to calculate spline interpolation with the following equation to interpolate between $y_i$ and $y_{i+1}$. + +$$ +Y_i(x) = a_i (x - x_i)^3 + b_i (x - x_i)^2 + c_i (x - x_i) + d_i \ \ \ (i = 0, \dots, N-1) +$$ + +Constraints on spline interpolation are as follows. +The number of constraints is $4N$, which is equal to the number of variables of spline interpolation. + +$$ +\begin{align} +Y_i (x_i) & = y_i \ \ \ (i = 0, \dots, N-1) \\ +Y_i (x_{i+1}) & = y_{i+1} \ \ \ (i = 0, \dots, N-1) \\ +Y'_i (x_{i+1}) & = Y'_{i+1} (x_{i+1}) \ \ \ (i = 0, \dots, N-2) \\ +Y''_i (x_{i+1}) & = Y''_{i+1} (x_{i+1}) \ \ \ (i = 0, \dots, N-2) \\ +Y''_0 (x_0) & = 0 \\ +Y''_{N-1} (x_N) & = 0 +\end{align} +$$ + +According to [this article](https://www.mk-mode.com/rails/docs/INTERPOLATION_SPLINE.pdf), spline interpolation is formulated as the following linear equation. + +$$ +\begin{align} + \begin{pmatrix} + 2(h_0 + h_1) & h_1 \\ + h_0 & 2 (h_1 + h_2) & h_2 & & O \\ + & & & \ddots \\ + O & & & & h_{N-2} & 2 (h_{N-2} + h_{N-1}) + \end{pmatrix} + \begin{pmatrix} + v_1 \\ v_2 \\ v_3 \\ \vdots \\ v_{N-1} + \end{pmatrix}= + \begin{pmatrix} + w_1 \\ w_2 \\ w_3 \\ \vdots \\ w_{N-1} + \end{pmatrix} +\end{align} +$$ + +where + +$$ +\begin{align} +h_i & = x_{i+1} - x_i \ \ \ (i = 0, \dots, N-1) \\ +w_i & = 6 \left(\frac{y_{i+1} - y_{i+1}}{h_i} - \frac{y_i - y_{i-1}}{h_{i-1}}\right) \ \ \ (i = 1, \dots, N-1) +\end{align} +$$ + +The coefficient matrix of this linear equation is tridiagonal matrix. Therefore, it can be solve with tridiagonal matrix algorithm, which can solve linear equations without gradient descent methods. + +Solving this linear equation with tridiagonal matrix algorithm, we can calculate coefficients of spline interpolation as follows. + +$$ +\begin{align} +a_i & = \frac{v_{i+1} - v_i}{6 (x_{i+1} - x_i)} \ \ \ (i = 0, \dots, N-1) \\ +b_i & = \frac{v_i}{2} \ \ \ (i = 0, \dots, N-1) \\ +c_i & = \frac{y_{i+1} - y_i}{x_{i+1} - x_i} - \frac{1}{6}(x_{i+1} - x_i)(2 v_i + v_{i+1}) \ \ \ (i = 0, \dots, N-1) \\ +d_i & = y_i \ \ \ (i = 0, \dots, N-1) +\end{align} +$$ + +### Tridiagonal Matrix Algorithm + +We solve tridiagonal linear equation according to [this article](https://www.iist.ac.in/sites/default/files/people/tdma.pdf) where variables of linear equation are expressed as follows in the implementation. + +$$ +\begin{align} + \begin{pmatrix} + b_0 & c_0 & & \\ + a_0 & b_1 & c_2 & O \\ + & & \ddots \\ + O & & a_{N-2} & b_{N-1} + \end{pmatrix} +x = +\begin{pmatrix} + d_0 \\ d_2 \\ d_3 \\ \vdots \\ d_{N-1} + \end{pmatrix} +\end{align} +$$ diff --git a/common/autoware_interpolation/include/autoware/interpolation/interpolation_utils.hpp b/common/autoware_interpolation/include/autoware/interpolation/interpolation_utils.hpp new file mode 100644 index 0000000000..1c0913c884 --- /dev/null +++ b/common/autoware_interpolation/include/autoware/interpolation/interpolation_utils.hpp @@ -0,0 +1,114 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_ +#define AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_ + +#include +#include +#include +#include + +namespace autoware::interpolation +{ +inline bool isIncreasing(const std::vector & x) +{ + if (x.empty()) { + throw std::invalid_argument("Points is empty."); + } + + for (size_t i = 0; i < x.size() - 1; ++i) { + if (x.at(i) >= x.at(i + 1)) { + return false; + } + } + + return true; +} + +inline bool isNotDecreasing(const std::vector & x) +{ + if (x.empty()) { + throw std::invalid_argument("Points is empty."); + } + + for (size_t i = 0; i < x.size() - 1; ++i) { + if (x.at(i) > x.at(i + 1)) { + return false; + } + } + + return true; +} + +inline std::vector validateKeys( + const std::vector & base_keys, const std::vector & query_keys) +{ + // when vectors are empty + if (base_keys.empty() || query_keys.empty()) { + throw std::invalid_argument("Points is empty."); + } + + // when size of vectors are less than 2 + if (base_keys.size() < 2) { + throw std::invalid_argument( + "The size of points is less than 2. base_keys.size() = " + std::to_string(base_keys.size())); + } + + // when indices are not sorted + if (!isIncreasing(base_keys) || !isNotDecreasing(query_keys)) { + throw std::invalid_argument("Either base_keys or query_keys is not sorted."); + } + + // when query_keys is out of base_keys (This function does not allow exterior division.) + constexpr double epsilon = 1e-3; + if ( + query_keys.front() < base_keys.front() - epsilon || + base_keys.back() + epsilon < query_keys.back()) { + throw std::invalid_argument("query_keys is out of base_keys"); + } + + // NOTE: Due to calculation error of double, a query key may be slightly out of base keys. + // Therefore, query keys are cropped here. + auto validated_query_keys = query_keys; + validated_query_keys.front() = std::max(validated_query_keys.front(), base_keys.front()); + validated_query_keys.back() = std::min(validated_query_keys.back(), base_keys.back()); + + return validated_query_keys; +} + +template +void validateKeysAndValues( + const std::vector & base_keys, const std::vector & base_values) +{ + // when vectors are empty + if (base_keys.empty() || base_values.empty()) { + throw std::invalid_argument("Points is empty."); + } + + // when size of vectors are less than 2 + if (base_keys.size() < 2 || base_values.size() < 2) { + throw std::invalid_argument( + "The size of points is less than 2. base_keys.size() = " + std::to_string(base_keys.size()) + + ", base_values.size() = " + std::to_string(base_values.size())); + } + + // when sizes of indices and values are not same + if (base_keys.size() != base_values.size()) { + throw std::invalid_argument("The size of base_keys and base_values are not the same."); + } +} +} // namespace autoware::interpolation + +#endif // AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_ diff --git a/common/autoware_interpolation/include/autoware/interpolation/linear_interpolation.hpp b/common/autoware_interpolation/include/autoware/interpolation/linear_interpolation.hpp new file mode 100644 index 0000000000..762806b3a5 --- /dev/null +++ b/common/autoware_interpolation/include/autoware/interpolation/linear_interpolation.hpp @@ -0,0 +1,35 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_ +#define AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_ + +#include "autoware/interpolation/interpolation_utils.hpp" + +#include + +namespace autoware::interpolation +{ +double lerp(const double src_val, const double dst_val, const double ratio); + +std::vector lerp( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & query_keys); + +double lerp( + const std::vector & base_keys, const std::vector & base_values, + const double query_key); +} // namespace autoware::interpolation + +#endif // AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_ diff --git a/common/autoware_interpolation/include/autoware/interpolation/spherical_linear_interpolation.hpp b/common/autoware_interpolation/include/autoware/interpolation/spherical_linear_interpolation.hpp new file mode 100644 index 0000000000..2e4a8fbd42 --- /dev/null +++ b/common/autoware_interpolation/include/autoware/interpolation/spherical_linear_interpolation.hpp @@ -0,0 +1,48 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ +#define AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ + +#include "autoware/interpolation/interpolation_utils.hpp" + +#include + +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +namespace autoware::interpolation +{ +geometry_msgs::msg::Quaternion slerp( + const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat, + const double ratio); + +std::vector slerp( + const std::vector & base_keys, + const std::vector & base_values, + const std::vector & query_keys); + +geometry_msgs::msg::Quaternion lerpOrientation( + const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to, + const double ratio); +} // namespace autoware::interpolation + +#endif // AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_ diff --git a/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp new file mode 100644 index 0000000000..f7feada78f --- /dev/null +++ b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp @@ -0,0 +1,97 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + +#include +#include +#include +#include +#include + +namespace autoware::interpolation +{ +// static spline interpolation functions +std::vector spline( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & query_keys); +std::vector splineByAkima( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & query_keys); + +// non-static 1-dimensional spline interpolation +// +// Usage: +// ``` +// SplineInterpolation spline; +// // memorize pre-interpolation result internally +// spline.calcSplineCoefficients(base_keys, base_values); +// const auto interpolation_result1 = spline.getSplineInterpolatedValues( +// base_keys, query_keys1); +// const auto interpolation_result2 = spline.getSplineInterpolatedValues( +// base_keys, query_keys2); +// ``` +class SplineInterpolation +{ +public: + SplineInterpolation() = default; + SplineInterpolation( + const std::vector & base_keys, const std::vector & base_values) + { + calcSplineCoefficients(base_keys, base_values); + } + + //!< @brief get values of spline interpolation on designated sampling points. + //!< @details Assuming that query_keys are t vector for sampling, and interpolation is for x, + // meaning that spline interpolation was applied to x(t), + // return value will be x(t) vector + std::vector getSplineInterpolatedValues(const std::vector & query_keys) const; + + //!< @brief get 1st differential values of spline interpolation on designated sampling points. + //!< @details Assuming that query_keys are t vector for sampling, and interpolation is for x, + // meaning that spline interpolation was applied to x(t), + // return value will be dx/dt(t) vector + std::vector getSplineInterpolatedDiffValues(const std::vector & query_keys) const; + + //!< @brief get 2nd differential values of spline interpolation on designated sampling points. + //!< @details Assuming that query_keys are t vector for sampling, and interpolation is for x, + // meaning that spline interpolation was applied to x(t), + // return value will be d^2/dt^2(t) vector + std::vector getSplineInterpolatedQuadDiffValues( + const std::vector & query_keys) const; + + size_t getSize() const { return base_keys_.size(); } + +private: + Eigen::VectorXd a_; + Eigen::VectorXd b_; + Eigen::VectorXd c_; + Eigen::VectorXd d_; + + std::vector base_keys_; + + void calcSplineCoefficients( + const std::vector & base_keys, const std::vector & base_values); + + Eigen::Index get_index(const double & key) const; +}; +} // namespace autoware::interpolation + +#endif // AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_ diff --git a/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp new file mode 100644 index 0000000000..c9668a4cad --- /dev/null +++ b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp @@ -0,0 +1,89 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ +#define AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ + +#include "autoware/interpolation/spline_interpolation.hpp" + +#include + +namespace autoware::interpolation +{ + +template +std::vector splineYawFromPoints(const std::vector & points); + +// non-static points spline interpolation +// NOTE: We can calculate yaw from the x and y by interpolation derivatives. +// +// Usage: +// ``` +// SplineInterpolationPoints2d spline; +// // memorize pre-interpolation result internally +// spline.calcSplineCoefficients(base_keys, base_values); +// const auto interpolation_result1 = spline.getSplineInterpolatedPoint( +// base_keys, query_keys1); +// const auto interpolation_result2 = spline.getSplineInterpolatedPoint( +// base_keys, query_keys2); +// const auto yaw_interpolation_result = spline.getSplineInterpolatedYaw( +// base_keys, query_keys1); +// ``` +class SplineInterpolationPoints2d +{ +public: + SplineInterpolationPoints2d() = default; + template + explicit SplineInterpolationPoints2d(const std::vector & points) + { + std::vector points_inner; + for (const auto & p : points) { + points_inner.push_back(autoware::universe_utils::getPoint(p)); + } + calcSplineCoefficientsInner(points_inner); + } + + // TODO(murooka) implement these functions + // std::vector getSplineInterpolatedPoints(const double width); + // std::vector getSplineInterpolatedPoses(const double width); + + // pose (= getSplineInterpolatedPoint + getSplineInterpolatedYaw) + geometry_msgs::msg::Pose getSplineInterpolatedPose(const size_t idx, const double s) const; + + // point + geometry_msgs::msg::Point getSplineInterpolatedPoint(const size_t idx, const double s) const; + + // yaw + double getSplineInterpolatedYaw(const size_t idx, const double s) const; + std::vector getSplineInterpolatedYaws() const; + + // curvature + double getSplineInterpolatedCurvature(const size_t idx, const double s) const; + std::vector getSplineInterpolatedCurvatures() const; + + size_t getSize() const { return base_s_vec_.size(); } + size_t getOffsetIndex(const size_t idx, const double offset) const; + double getAccumulatedLength(const size_t idx) const; + +private: + void calcSplineCoefficientsInner(const std::vector & points); + SplineInterpolation spline_x_; + SplineInterpolation spline_y_; + SplineInterpolation spline_z_; + + std::vector base_s_vec_; +}; +} // namespace autoware::interpolation + +#endif // AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_ diff --git a/common/autoware_interpolation/include/autoware/interpolation/zero_order_hold.hpp b/common/autoware_interpolation/include/autoware/interpolation/zero_order_hold.hpp new file mode 100644 index 0000000000..c28f870837 --- /dev/null +++ b/common/autoware_interpolation/include/autoware/interpolation/zero_order_hold.hpp @@ -0,0 +1,81 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__INTERPOLATION__ZERO_ORDER_HOLD_HPP_ +#define AUTOWARE__INTERPOLATION__ZERO_ORDER_HOLD_HPP_ + +#include "autoware/interpolation/interpolation_utils.hpp" + +#include + +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 = validateKeys(base_keys, query_keys); + + std::vector closest_segment_indices(validated_query_keys.size()); + size_t closest_segment_idx = 0; + for (size_t i = 0; i < validated_query_keys.size(); ++i) { + // Check if query_key is closes to the terminal point of the base keys + if (base_keys.back() - overlap_threshold < validated_query_keys.at(i)) { + closest_segment_idx = base_keys.size() - 1; + } else { + for (size_t j = base_keys.size() - 1; j > closest_segment_idx; --j) { + if ( + base_keys.at(j - 1) - overlap_threshold < validated_query_keys.at(i) && + validated_query_keys.at(i) < base_keys.at(j)) { + // find closest segment in base keys + closest_segment_idx = j - 1; + break; + } + } + } + + closest_segment_indices.at(i) = closest_segment_idx; + } + + return closest_segment_indices; +} + +template +std::vector zero_order_hold( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & closest_segment_indices) +{ + // throw exception for invalid arguments + validateKeysAndValues(base_keys, base_values); + + std::vector query_values(closest_segment_indices.size()); + for (size_t i = 0; i < closest_segment_indices.size(); ++i) { + query_values.at(i) = base_values.at(closest_segment_indices.at(i)); + } + + return query_values; +} + +template +std::vector zero_order_hold( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & query_keys, const double overlap_threshold = 1e-3) +{ + return zero_order_hold( + base_keys, base_values, calc_closest_segment_indices(base_keys, query_keys, overlap_threshold)); +} +} // namespace autoware::interpolation + +#endif // AUTOWARE__INTERPOLATION__ZERO_ORDER_HOLD_HPP_ diff --git a/common/autoware_interpolation/package.xml b/common/autoware_interpolation/package.xml new file mode 100644 index 0000000000..3bad757405 --- /dev/null +++ b/common/autoware_interpolation/package.xml @@ -0,0 +1,24 @@ + + + + autoware_interpolation + 0.40.0 + The spline interpolation package + Fumiya Watanabe + Takayuki Murooka + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_universe_utils + eigen + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/autoware_interpolation/src/linear_interpolation.cpp b/common/autoware_interpolation/src/linear_interpolation.cpp new file mode 100644 index 0000000000..4ba339f0e5 --- /dev/null +++ b/common/autoware_interpolation/src/linear_interpolation.cpp @@ -0,0 +1,59 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/interpolation/linear_interpolation.hpp" + +#include + +namespace autoware::interpolation +{ +double lerp(const double src_val, const double dst_val, const double ratio) +{ + return src_val + (dst_val - src_val) * ratio; +} + +std::vector lerp( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & query_keys) +{ + // throw exception for invalid arguments + const auto validated_query_keys = validateKeys(base_keys, query_keys); + validateKeysAndValues(base_keys, base_values); + + // calculate linear interpolation + std::vector query_values; + size_t key_index = 0; + for (const auto query_key : validated_query_keys) { + while (base_keys.at(key_index + 1) < query_key) { + ++key_index; + } + + const double src_val = base_values.at(key_index); + const double dst_val = base_values.at(key_index + 1); + const double ratio = (query_key - base_keys.at(key_index)) / + (base_keys.at(key_index + 1) - base_keys.at(key_index)); + + const double interpolated_val = lerp(src_val, dst_val, ratio); + query_values.push_back(interpolated_val); + } + + return query_values; +} + +double lerp( + const std::vector & base_keys, const std::vector & base_values, double query_key) +{ + return lerp(base_keys, base_values, std::vector{query_key}).front(); +} +} // namespace autoware::interpolation diff --git a/common/autoware_interpolation/src/spherical_linear_interpolation.cpp b/common/autoware_interpolation/src/spherical_linear_interpolation.cpp new file mode 100644 index 0000000000..5acdf6a903 --- /dev/null +++ b/common/autoware_interpolation/src/spherical_linear_interpolation.cpp @@ -0,0 +1,73 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/interpolation/spherical_linear_interpolation.hpp" + +#include + +namespace autoware::interpolation +{ +geometry_msgs::msg::Quaternion slerp( + const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat, + const double ratio) +{ + tf2::Quaternion src_tf; + tf2::Quaternion dst_tf; + tf2::fromMsg(src_quat, src_tf); + tf2::fromMsg(dst_quat, dst_tf); + const auto interpolated_quat = tf2::slerp(src_tf, dst_tf, ratio); + return tf2::toMsg(interpolated_quat); +} + +std::vector slerp( + const std::vector & base_keys, + const std::vector & base_values, + const std::vector & query_keys) +{ + // throw exception for invalid arguments + const auto validated_query_keys = validateKeys(base_keys, query_keys); + validateKeysAndValues(base_keys, base_values); + + // calculate linear interpolation + std::vector query_values; + size_t key_index = 0; + for (const auto query_key : validated_query_keys) { + while (base_keys.at(key_index + 1) < query_key) { + ++key_index; + } + + const auto src_quat = base_values.at(key_index); + const auto dst_quat = base_values.at(key_index + 1); + const double ratio = (query_key - base_keys.at(key_index)) / + (base_keys.at(key_index + 1) - base_keys.at(key_index)); + + const auto interpolated_quat = slerp(src_quat, dst_quat, ratio); + query_values.push_back(interpolated_quat); + } + + return query_values; +} + +geometry_msgs::msg::Quaternion lerpOrientation( + const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to, + const double ratio) +{ + tf2::Quaternion q_from, q_to; + tf2::fromMsg(o_from, q_from); + tf2::fromMsg(o_to, q_to); + + const auto q_interpolated = q_from.slerp(q_to, ratio); + return tf2::toMsg(q_interpolated); +} +} // namespace autoware::interpolation diff --git a/common/autoware_interpolation/src/spline_interpolation.cpp b/common/autoware_interpolation/src/spline_interpolation.cpp new file mode 100644 index 0000000000..78f3778ae8 --- /dev/null +++ b/common/autoware_interpolation/src/spline_interpolation.cpp @@ -0,0 +1,247 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/interpolation/spline_interpolation.hpp" + +#include +#include + +namespace autoware::interpolation +{ +Eigen::VectorXd solve_tridiagonal_matrix_algorithm( + const Eigen::Ref & a, const Eigen::Ref & b, + const Eigen::Ref & c, const Eigen::Ref & d) +{ + const auto n = d.size(); + + if (n == 1) { + return d.array() / b.array(); + } + + 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); + + 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; + } + + // 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; +} + +std::vector spline( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & query_keys) +{ + // calculate spline coefficients + SplineInterpolation interpolator(base_keys, base_values); + + // interpolate base_keys at query_keys + return interpolator.getSplineInterpolatedValues(query_keys); +} + +std::vector splineByAkima( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & query_keys) +{ + constexpr double epsilon = 1e-5; + + // calculate m + std::vector m_values; + for (size_t i = 0; i < base_keys.size() - 1; ++i) { + const double m_val = + (base_values.at(i + 1) - base_values.at(i)) / (base_keys.at(i + 1) - base_keys.at(i)); + m_values.push_back(m_val); + } + + // calculate s + std::vector s_values; + for (size_t i = 0; i < base_keys.size(); ++i) { + if (i == 0) { + s_values.push_back(m_values.front()); + continue; + } else if (i == base_keys.size() - 1) { + s_values.push_back(m_values.back()); + continue; + } else if (i == 1 || i == base_keys.size() - 2) { + const double s_val = (m_values.at(i - 1) + m_values.at(i)) / 2.0; + s_values.push_back(s_val); + continue; + } + + const double denom = std::abs(m_values.at(i + 1) - m_values.at(i)) + + std::abs(m_values.at(i - 1) - m_values.at(i - 2)); + if (std::abs(denom) < epsilon) { + const double s_val = (m_values.at(i - 1) + m_values.at(i)) / 2.0; + s_values.push_back(s_val); + continue; + } + + const double s_val = (std::abs(m_values.at(i + 1) - m_values.at(i)) * m_values.at(i - 1) + + std::abs(m_values.at(i - 1) - m_values.at(i - 2)) * m_values.at(i)) / + denom; + s_values.push_back(s_val); + } + + // calculate cubic coefficients + std::vector a; + std::vector b; + std::vector c; + std::vector d; + for (size_t i = 0; i < base_keys.size() - 1; ++i) { + a.push_back( + (s_values.at(i) + s_values.at(i + 1) - 2.0 * m_values.at(i)) / + std::pow(base_keys.at(i + 1) - base_keys.at(i), 2)); + b.push_back( + (3.0 * m_values.at(i) - 2.0 * s_values.at(i) - s_values.at(i + 1)) / + (base_keys.at(i + 1) - base_keys.at(i))); + c.push_back(s_values.at(i)); + d.push_back(base_values.at(i)); + } + + // interpolate + std::vector res; + size_t j = 0; + for (const auto & query_key : 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 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 + 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( + 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; +} + +std::vector SplineInterpolation::getSplineInterpolatedValues( + const std::vector & query_keys) const +{ + // throw exceptions for invalid arguments + const auto validated_query_keys = autoware::interpolation::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 interpolated_values; +} + +std::vector SplineInterpolation::getSplineInterpolatedDiffValues( + const std::vector & query_keys) const +{ + // throw exceptions for invalid arguments + const auto validated_query_keys = autoware::interpolation::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 interpolated_diff_values; +} + +std::vector SplineInterpolation::getSplineInterpolatedQuadDiffValues( + const std::vector & query_keys) const +{ + // throw exceptions for invalid arguments + 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()); + + 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 interpolated_quad_diff_values; +} +} // namespace autoware::interpolation diff --git a/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp b/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp new file mode 100644 index 0000000000..e0f55cfb24 --- /dev/null +++ b/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp @@ -0,0 +1,212 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" + +#include + +namespace autoware::interpolation +{ +std::vector calcEuclidDist(const std::vector & x, const std::vector & y) +{ + if (x.size() != y.size()) { + return std::vector{}; + } + + std::vector dist_v; + dist_v.push_back(0.0); + for (size_t i = 0; i < x.size() - 1; ++i) { + const double dx = x.at(i + 1) - x.at(i); + const double dy = y.at(i + 1) - y.at(i); + dist_v.push_back(dist_v.at(i) + std::hypot(dx, dy)); + } + + return dist_v; +} + +std::array, 4> getBaseValues( + const std::vector & points) +{ + // calculate x, y + std::vector base_x; + std::vector base_y; + std::vector base_z; + for (size_t i = 0; i < points.size(); i++) { + const auto & current_pos = points.at(i); + if (i > 0) { + const auto & prev_pos = points.at(i - 1); + if ( + std::fabs(current_pos.x - prev_pos.x) < 1e-6 && + std::fabs(current_pos.y - prev_pos.y) < 1e-6) { + continue; + } + } + base_x.push_back(current_pos.x); + base_y.push_back(current_pos.y); + base_z.push_back(current_pos.z); + } + + // calculate base_keys, base_values + if (base_x.size() < 2 || base_y.size() < 2 || base_z.size() < 2) { + throw std::logic_error("The number of unique points is not enough."); + } + + const std::vector base_s = calcEuclidDist(base_x, base_y); + + return {base_s, base_x, base_y, base_z}; +} + +template +std::vector splineYawFromPoints(const std::vector & points) +{ + // calculate spline coefficients + SplineInterpolationPoints2d interpolator(points); + + // interpolate base_keys at query_keys + std::vector yaw_vec; + for (size_t i = 0; i < points.size(); ++i) { + const double yaw = interpolator.getSplineInterpolatedYaw(i, 0.0); + yaw_vec.push_back(yaw); + } + return yaw_vec; +} +template std::vector splineYawFromPoints( + const std::vector & points); + +geometry_msgs::msg::Pose SplineInterpolationPoints2d::getSplineInterpolatedPose( + const size_t idx, const double s) const +{ + geometry_msgs::msg::Pose pose; + pose.position = getSplineInterpolatedPoint(idx, s); + pose.orientation = + autoware::universe_utils::createQuaternionFromYaw(getSplineInterpolatedYaw(idx, s)); + return pose; +} + +geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoint( + const size_t idx, const double s) const +{ + if (base_s_vec_.size() <= idx) { + throw std::out_of_range("idx is out of range."); + } + + double whole_s = base_s_vec_.at(idx) + s; + if (whole_s < base_s_vec_.front()) { + whole_s = base_s_vec_.front(); + } + if (whole_s > base_s_vec_.back()) { + whole_s = base_s_vec_.back(); + } + + const double x = spline_x_.getSplineInterpolatedValues({whole_s}).at(0); + const double y = spline_y_.getSplineInterpolatedValues({whole_s}).at(0); + const double z = spline_z_.getSplineInterpolatedValues({whole_s}).at(0); + + geometry_msgs::msg::Point geom_point; + geom_point.x = x; + geom_point.y = y; + geom_point.z = z; + return geom_point; +} + +double SplineInterpolationPoints2d::getSplineInterpolatedYaw(const size_t idx, const double s) const +{ + if (base_s_vec_.size() <= idx) { + throw std::out_of_range("idx is out of range."); + } + + const double whole_s = + std::clamp(base_s_vec_.at(idx) + s, base_s_vec_.front(), base_s_vec_.back()); + + const double diff_x = spline_x_.getSplineInterpolatedDiffValues({whole_s}).at(0); + const double diff_y = spline_y_.getSplineInterpolatedDiffValues({whole_s}).at(0); + + return std::atan2(diff_y, diff_x); +} + +std::vector SplineInterpolationPoints2d::getSplineInterpolatedYaws() const +{ + std::vector yaw_vec; + for (size_t i = 0; i < spline_x_.getSize(); ++i) { + const double yaw = getSplineInterpolatedYaw(i, 0.0); + yaw_vec.push_back(yaw); + } + return yaw_vec; +} + +double SplineInterpolationPoints2d::getSplineInterpolatedCurvature( + const size_t idx, const double s) const +{ + if (base_s_vec_.size() <= idx) { + throw std::out_of_range("idx is out of range."); + } + + const double whole_s = + std::clamp(base_s_vec_.at(idx) + s, base_s_vec_.front(), base_s_vec_.back()); + + const double diff_x = spline_x_.getSplineInterpolatedDiffValues({whole_s}).at(0); + const double diff_y = spline_y_.getSplineInterpolatedDiffValues({whole_s}).at(0); + + const double quad_diff_x = spline_x_.getSplineInterpolatedQuadDiffValues({whole_s}).at(0); + const double quad_diff_y = spline_y_.getSplineInterpolatedQuadDiffValues({whole_s}).at(0); + + return (diff_x * quad_diff_y - quad_diff_x * diff_y) / + std::pow(std::pow(diff_x, 2) + std::pow(diff_y, 2), 1.5); +} + +std::vector SplineInterpolationPoints2d::getSplineInterpolatedCurvatures() const +{ + std::vector curvature_vec; + for (size_t i = 0; i < spline_x_.getSize(); ++i) { + const double curvature = getSplineInterpolatedCurvature(i, 0.0); + curvature_vec.push_back(curvature); + } + return curvature_vec; +} + +size_t SplineInterpolationPoints2d::getOffsetIndex(const size_t idx, const double offset) const +{ + const double whole_s = base_s_vec_.at(idx) + offset; + for (size_t s_idx = 0; s_idx < base_s_vec_.size(); ++s_idx) { + if (whole_s < base_s_vec_.at(s_idx)) { + return s_idx; + } + } + return base_s_vec_.size() - 1; +} + +double SplineInterpolationPoints2d::getAccumulatedLength(const size_t idx) const +{ + if (base_s_vec_.size() <= idx) { + throw std::out_of_range("idx is out of range."); + } + return base_s_vec_.at(idx); +} + +void SplineInterpolationPoints2d::calcSplineCoefficientsInner( + const std::vector & points) +{ + const auto base = getBaseValues(points); + + base_s_vec_ = base.at(0); + const auto & base_x_vec = base.at(1); + const auto & base_y_vec = base.at(2); + const auto & base_z_vec = base.at(3); + + // calculate spline coefficients + spline_x_ = SplineInterpolation(base_s_vec_, base_x_vec); + 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/autoware_interpolation/test/src/test_interpolation.cpp b/common/autoware_interpolation/test/src/test_interpolation.cpp new file mode 100644 index 0000000000..ebb93a0a85 --- /dev/null +++ b/common/autoware_interpolation/test/src/test_interpolation.cpp @@ -0,0 +1,21 @@ +// Copyright 2021 TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/autoware_interpolation/test/src/test_interpolation_utils.cpp b/common/autoware_interpolation/test/src/test_interpolation_utils.cpp new file mode 100644 index 0000000000..9131b458dd --- /dev/null +++ b/common/autoware_interpolation/test/src/test_interpolation_utils.cpp @@ -0,0 +1,138 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/interpolation/interpolation_utils.hpp" + +#include + +#include + +TEST(interpolation_utils, isIncreasing) +{ + // empty + const std::vector empty_vec; + 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(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(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(autoware::interpolation::isIncreasing(decreasing_vec), false); +} + +TEST(interpolation_utils, isNotDecreasing) +{ + // empty + const std::vector empty_vec; + 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(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(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(autoware::interpolation::isNotDecreasing(decreasing_vec), false); +} + +TEST(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}; + + // valid + EXPECT_NO_THROW(validateKeys(base_keys, query_keys)); + + // empty + const std::vector empty_vec; + EXPECT_THROW(validateKeys(empty_vec, query_keys), std::invalid_argument); + EXPECT_THROW(validateKeys(base_keys, empty_vec), std::invalid_argument); + + // size is less than 2 + const std::vector short_vec{0.0}; + EXPECT_THROW(validateKeys(short_vec, query_keys), std::invalid_argument); + + // partly not increase + const std::vector partly_not_increasing_vec{0.0, 0.0, 2.0, 3.0}; + // NOTE: base_keys must be strictly monotonous increasing vector + EXPECT_THROW(validateKeys(partly_not_increasing_vec, query_keys), std::invalid_argument); + // NOTE: query_keys is allowed to be monotonous non-decreasing vector + EXPECT_NO_THROW(validateKeys(base_keys, partly_not_increasing_vec)); + + // decrease + const std::vector decreasing_vec{0.0, -1.0, 2.0, 3.0}; + EXPECT_THROW(validateKeys(decreasing_vec, query_keys), std::invalid_argument); + EXPECT_THROW(validateKeys(base_keys, decreasing_vec), std::invalid_argument); + + // out of range + const std::vector front_out_query_keys{-1.0, 1.0, 2.0, 3.0}; + EXPECT_THROW(validateKeys(base_keys, front_out_query_keys), std::invalid_argument); + + const std::vector back_out_query_keys{0.0, 1.0, 2.0, 4.0}; + EXPECT_THROW(validateKeys(base_keys, back_out_query_keys), std::invalid_argument); + + { // validated key check in normal case + const std::vector normal_query_keys{0.5, 1.5, 3.0}; + const auto validated_query_keys = validateKeys(base_keys, normal_query_keys); + for (size_t i = 0; i < normal_query_keys.size(); ++i) { + EXPECT_EQ(normal_query_keys.at(i), validated_query_keys.at(i)); + } + } + + { // validated key check in case slightly out of range + constexpr double slightly_out_of_range_epsilon = 1e-6; + const std::vector slightly_out_of_range__query_keys{ + 0.0 - slightly_out_of_range_epsilon, 3.0 + slightly_out_of_range_epsilon}; + const auto validated_query_keys = validateKeys(base_keys, slightly_out_of_range__query_keys); + EXPECT_NEAR(validated_query_keys.at(0), 0.0, 1e-10); + EXPECT_NEAR(validated_query_keys.at(1), 3.0, 1e-10); + } +} + +TEST(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}; + + // valid + EXPECT_NO_THROW(validateKeysAndValues(base_keys, base_values)); + + // empty + const std::vector empty_vec; + EXPECT_THROW(validateKeysAndValues(empty_vec, base_values), std::invalid_argument); + EXPECT_THROW(validateKeysAndValues(base_keys, empty_vec), std::invalid_argument); + + // size is less than 2 + const std::vector short_vec{0.0}; + EXPECT_THROW(validateKeysAndValues(short_vec, base_values), std::invalid_argument); + EXPECT_THROW(validateKeysAndValues(base_keys, short_vec), std::invalid_argument); + + // size is different + const std::vector different_size_base_values{0.0, 1.0, 2.0}; + EXPECT_THROW(validateKeysAndValues(base_keys, different_size_base_values), std::invalid_argument); +} diff --git a/common/autoware_interpolation/test/src/test_linear_interpolation.cpp b/common/autoware_interpolation/test/src/test_linear_interpolation.cpp new file mode 100644 index 0000000000..0fea1e514e --- /dev/null +++ b/common/autoware_interpolation/test/src/test_linear_interpolation.cpp @@ -0,0 +1,95 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/interpolation/linear_interpolation.hpp" + +#include + +#include +#include + +constexpr double epsilon = 1e-6; + +TEST(linear_interpolation, lerp_scalar) +{ + 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) +{ + { // straight: query_keys is same as base_keys + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector base_values{0.0, 1.5, 3.0, 4.5, 6.0}; + const std::vector query_keys = base_keys; + const std::vector ans = base_values; + + 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); + } + } + + { // straight: query_keys is random + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector base_values{0.0, 1.5, 3.0, 4.5, 6.0}; + 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 = 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); + } + } + + { // curve: query_keys is same as base_keys + 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 = base_keys; + const std::vector ans = base_values; + + 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); + } + } + + { // curve: query_keys is same as random + const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; + const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; + const std::vector query_keys{0.0, 8.0, 18.0}; + const std::vector ans{-0.18, 1.12, 1.4}; + + 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); + } + } +} + +TEST(linear_interpolation, lerp_scalar_query) +{ + { // curve: query_keys is same as random + const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; + const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; + const std::vector query_keys{0.0, 8.0, 18.0}; + const std::vector ans{-0.18, 1.12, 1.4}; + + for (size_t i = 0; i < query_keys.size(); ++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/autoware_interpolation/test/src/test_spherical_linear_interpolation.cpp b/common/autoware_interpolation/test/src/test_spherical_linear_interpolation.cpp new file mode 100644 index 0000000000..b7ce134c68 --- /dev/null +++ b/common/autoware_interpolation/test/src/test_spherical_linear_interpolation.cpp @@ -0,0 +1,137 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/interpolation/spherical_linear_interpolation.hpp" + +#include + +#include +#include + +constexpr double epsilon = 1e-6; + +namespace +{ +inline geometry_msgs::msg::Quaternion createQuaternionFromRPY( + const double roll, const double pitch, const double yaw) +{ + tf2::Quaternion q; + q.setRPY(roll, pitch, yaw); + return tf2::toMsg(q); +} +} // namespace + +TEST(slerp, spline_scalar) +{ + using autoware::interpolation::slerp; + + // Same value + { + const double src_yaw = 0.0; + const double dst_yaw = 0.0; + const auto src_quat = createQuaternionFromRPY(0.0, 0.0, src_yaw); + const auto dst_quat = createQuaternionFromRPY(0.0, 0.0, dst_yaw); + + const auto ans_quat = createQuaternionFromRPY(0.0, 0.0, 0.0); + + for (double ratio = -2.0; ratio < 2.0 + epsilon; ratio += 0.1) { + const auto interpolated_quat = slerp(src_quat, dst_quat, ratio); + EXPECT_NEAR(ans_quat.x, interpolated_quat.x, epsilon); + EXPECT_NEAR(ans_quat.y, interpolated_quat.y, epsilon); + EXPECT_NEAR(ans_quat.z, interpolated_quat.z, epsilon); + EXPECT_NEAR(ans_quat.w, interpolated_quat.w, epsilon); + } + } + + // Random Value + { + const double src_yaw = 0.0; + const double dst_yaw = M_PI; + const auto src_quat = createQuaternionFromRPY(0.0, 0.0, src_yaw); + const auto dst_quat = createQuaternionFromRPY(0.0, 0.0, dst_yaw); + + for (double ratio = -2.0; ratio < 2.0 + epsilon; ratio += 0.1) { + const auto interpolated_quat = slerp(src_quat, dst_quat, ratio); + + const double ans_yaw = M_PI * ratio; + tf2::Quaternion ans; + ans.setRPY(0, 0, ans_yaw); + const geometry_msgs::msg::Quaternion ans_quat = tf2::toMsg(ans); + + EXPECT_NEAR(ans_quat.x, interpolated_quat.x, epsilon); + EXPECT_NEAR(ans_quat.y, interpolated_quat.y, epsilon); + EXPECT_NEAR(ans_quat.z, interpolated_quat.z, epsilon); + EXPECT_NEAR(ans_quat.w, interpolated_quat.w, epsilon); + } + } +} + +TEST(slerp, spline_vector) +{ + using autoware::interpolation::slerp; + + // query keys are same as base keys + { + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + std::vector base_values; + for (size_t i = 0; i < 5; ++i) { + const auto quat = createQuaternionFromRPY(0.0, 0.0, i * M_PI / 5.0); + base_values.push_back(quat); + } + const std::vector query_keys = base_keys; + const auto ans = base_values; + + const auto results = slerp(base_keys, base_values, query_keys); + + for (size_t i = 0; i < results.size(); ++i) { + const auto interpolated_quat = results.at(i); + const auto ans_quat = ans.at(i); + + EXPECT_NEAR(ans_quat.x, interpolated_quat.x, epsilon); + EXPECT_NEAR(ans_quat.y, interpolated_quat.y, epsilon); + EXPECT_NEAR(ans_quat.z, interpolated_quat.z, epsilon); + EXPECT_NEAR(ans_quat.w, interpolated_quat.w, epsilon); + } + } + + // random + { + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + std::vector base_values; + for (size_t i = 0; i < 5; ++i) { + const auto quat = createQuaternionFromRPY(0.0, 0.0, i * M_PI / 5.0); + base_values.push_back(quat); + } + const std::vector query_keys = {0.0, 0.1, 1.5, 2.6, 3.1, 3.8}; + std::vector ans(query_keys.size()); + ans.at(0) = createQuaternionFromRPY(0.0, 0.0, 0.0); + ans.at(1) = createQuaternionFromRPY(0.0, 0.0, 0.1 * M_PI / 5.0); + ans.at(2) = createQuaternionFromRPY(0.0, 0.0, 0.5 * M_PI / 5.0 + M_PI / 5.0); + ans.at(3) = createQuaternionFromRPY(0.0, 0.0, 0.6 * M_PI / 5.0 + 2.0 * M_PI / 5.0); + ans.at(4) = createQuaternionFromRPY(0.0, 0.0, 0.1 * M_PI / 5.0 + 3.0 * M_PI / 5.0); + ans.at(5) = createQuaternionFromRPY(0.0, 0.0, 0.8 * M_PI / 5.0 + 3.0 * M_PI / 5.0); + + const auto results = slerp(base_keys, base_values, query_keys); + + for (size_t i = 0; i < results.size(); ++i) { + const auto interpolated_quat = results.at(i); + const auto ans_quat = ans.at(i); + + EXPECT_NEAR(ans_quat.x, interpolated_quat.x, epsilon); + EXPECT_NEAR(ans_quat.y, interpolated_quat.y, epsilon); + EXPECT_NEAR(ans_quat.z, interpolated_quat.z, epsilon); + EXPECT_NEAR(ans_quat.w, interpolated_quat.w, epsilon); + } + } +} diff --git a/common/autoware_interpolation/test/src/test_spline_interpolation.cpp b/common/autoware_interpolation/test/src/test_spline_interpolation.cpp new file mode 100644 index 0000000000..766e94a47b --- /dev/null +++ b/common/autoware_interpolation/test/src/test_spline_interpolation.cpp @@ -0,0 +1,279 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" + +#include + +#include +#include + +constexpr double epsilon = 1e-6; + +using autoware::interpolation::SplineInterpolation; + +TEST(spline_interpolation, spline) +{ + { // straight: query_keys is same as base_keys + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector base_values{0.0, 1.5, 3.0, 4.5, 6.0}; + const std::vector query_keys = base_keys; + const std::vector ans = base_values; + + 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); + } + } + + { // straight: query_keys is random + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector base_values{0.0, 1.5, 3.0, 4.5, 6.0}; + 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 = 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); + } + } + + { // curve: query_keys is same as base_keys + 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 = base_keys; + const std::vector ans = base_values; + + 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); + } + } + + { // curve: query_keys is random + const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; + const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; + const std::vector query_keys{0.0, 8.0, 18.0}; + const std::vector ans{-0.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) { + EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + } + } + + { // straight: size of base_keys is 2 (edge case in the implementation) + const std::vector base_keys{0.0, 1.0}; + const std::vector base_values{0.0, 1.5}; + const std::vector query_keys = base_keys; + const std::vector ans = base_values; + + 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); + } + } + + { // straight: size of base_keys is 3 (edge case in the implementation) + const std::vector base_keys{0.0, 1.0, 2.0}; + const std::vector base_values{0.0, 1.5, 3.0}; + const std::vector query_keys = base_keys; + const std::vector ans = base_values; + + 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); + } + } + + { // curve: query_keys is random. size of base_keys is 3 (edge case in the implementation) + const std::vector base_keys{-1.5, 1.0, 5.0}; + const std::vector base_values{-1.2, 0.5, 1.0}; + 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 = 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); + } + } + + { // When the query keys changes suddenly (edge case of spline interpolation). + 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 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); + } + } +} + +TEST(spline_interpolation, splineByAkima) +{ + { // straight: query_keys is same as base_keys + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector base_values{0.0, 1.5, 3.0, 4.5, 6.0}; + const std::vector query_keys = base_keys; + const std::vector ans = base_values; + + 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); + } + } + + { // straight: query_keys is random + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector base_values{0.0, 1.5, 3.0, 4.5, 6.0}; + 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 = + 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); + } + } + + { // curve: query_keys is same as base_keys + 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 = base_keys; + const std::vector ans = base_values; + + 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); + } + } + + { // curve: query_keys is random + const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; + const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; + const std::vector query_keys{0.0, 8.0, 18.0}; + const std::vector ans{-0.0801, 1.110749, 1.4864}; + + 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); + } + } + + { // straight: size of base_keys is 2 (edge case in the implementation) + const std::vector base_keys{0.0, 1.0}; + const std::vector base_values{0.0, 1.5}; + const std::vector query_keys = base_keys; + const std::vector ans = base_values; + + 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); + } + } + + { // straight: size of base_keys is 3 (edge case in the implementation) + const std::vector base_keys{0.0, 1.0, 2.0}; + const std::vector base_values{0.0, 1.5, 3.0}; + const std::vector query_keys = base_keys; + const std::vector ans = base_values; + + 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); + } + } + + { // curve: query_keys is random. size of base_keys is 3 (edge case in the implementation) + const std::vector base_keys{-1.5, 1.0, 5.0}; + const std::vector base_values{-1.2, 0.5, 1.0}; + 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 = + 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); + } + } + + { // When the query keys changes suddenly (edge case of spline interpolation). + 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, 0.1, 0.1, 0.1, 0.1}; + + 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); + } + } +} + +TEST(spline_interpolation, SplineInterpolation) +{ + { + // curve: query_keys is random + const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; + const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; + const std::vector query_keys{0.0, 8.0, 18.0}; + const std::vector ans{-0.076114, 1.001217, 1.573640}; + + SplineInterpolation s(base_keys, base_values); + const std::vector query_values = s.getSplineInterpolatedValues(query_keys); + + for (size_t i = 0; i < query_values.size(); ++i) { + EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + } + } + + { + // getSplineInterpolatedDiffValues + 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}; + + SplineInterpolation s(base_keys, base_values); + const std::vector query_values = s.getSplineInterpolatedDiffValues(query_keys); + + for (size_t i = 0; i < query_values.size(); ++i) { + EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + } + } + + { + // getSplineInterpolatedQuadDiffValues + 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}; + + SplineInterpolation s(base_keys, base_values); + const std::vector query_values = s.getSplineInterpolatedQuadDiffValues(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_interpolation/test/src/test_spline_interpolation_points_2d.cpp b/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp new file mode 100644 index 0000000000..974ad606b9 --- /dev/null +++ b/common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp @@ -0,0 +1,223 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" + +#include + +#include +#include + +constexpr double epsilon = 1e-6; + +using autoware::interpolation::SplineInterpolationPoints2d; + +TEST(spline_interpolation, splineYawFromPoints) +{ + using autoware::universe_utils::createPoint; + + { // straight + std::vector points; + points.push_back(createPoint(0.0, 0.0, 0.0)); + points.push_back(createPoint(1.0, 1.5, 0.0)); + points.push_back(createPoint(2.0, 3.0, 0.0)); + points.push_back(createPoint(3.0, 4.5, 0.0)); + points.push_back(createPoint(4.0, 6.0, 0.0)); + + const std::vector ans{0.9827937, 0.9827937, 0.9827937, 0.9827937, 0.9827937}; + + const auto yaws = autoware::interpolation::splineYawFromPoints(points); + for (size_t i = 0; i < yaws.size(); ++i) { + EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); + } + } + + { // curve + std::vector points; + points.push_back(createPoint(-2.0, -10.0, 0.0)); + points.push_back(createPoint(2.0, 1.5, 0.0)); + points.push_back(createPoint(3.0, 3.0, 0.0)); + points.push_back(createPoint(5.0, 10.0, 0.0)); + points.push_back(createPoint(10.0, 12.5, 0.0)); + + const std::vector ans{1.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); + } + } + + { // size of base_keys is 1 (infeasible to interpolate) + std::vector points; + points.push_back(createPoint(1.0, 0.0, 0.0)); + + EXPECT_THROW(autoware::interpolation::splineYawFromPoints(points), std::logic_error); + } + + { // straight: size of base_keys is 2 (edge case in the implementation) + std::vector points; + points.push_back(createPoint(1.0, 0.0, 0.0)); + points.push_back(createPoint(2.0, 1.5, 0.0)); + + const std::vector ans{0.9827937, 0.9827937}; + + const auto yaws = autoware::interpolation::splineYawFromPoints(points); + for (size_t i = 0; i < yaws.size(); ++i) { + EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); + } + } + + { // straight: size of base_keys is 3 (edge case in the implementation) + std::vector points; + points.push_back(createPoint(1.0, 0.0, 0.0)); + points.push_back(createPoint(2.0, 1.5, 0.0)); + points.push_back(createPoint(3.0, 3.0, 0.0)); + + const std::vector ans{0.9827937, 0.9827937, 0.9827937}; + + const auto yaws = autoware::interpolation::splineYawFromPoints(points); + for (size_t i = 0; i < yaws.size(); ++i) { + EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); + } + } +} + +TEST(spline_interpolation, SplineInterpolationPoints2d) +{ + using autoware::universe_utils::createPoint; + + // curve + std::vector points; + points.push_back(createPoint(-2.0, -10.0, 0.0)); + points.push_back(createPoint(2.0, 1.5, 0.0)); + points.push_back(createPoint(3.0, 3.0, 0.0)); + points.push_back(createPoint(5.0, 10.0, 0.0)); + points.push_back(createPoint(10.0, 12.5, 0.0)); + + SplineInterpolationPoints2d s(points); + + { // point + // front + const auto front_point = s.getSplineInterpolatedPoint(0, 0.0); + EXPECT_NEAR(front_point.x, -2.0, epsilon); + EXPECT_NEAR(front_point.y, -10.0, epsilon); + + // back + const auto back_point = s.getSplineInterpolatedPoint(4, 0.0); + EXPECT_NEAR(back_point.x, 10.0, epsilon); + EXPECT_NEAR(back_point.y, 12.5, epsilon); + + // random + const auto random_point = s.getSplineInterpolatedPoint(3, 0.5); + EXPECT_NEAR(random_point.x, 5.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); + EXPECT_NEAR(front_out_point.x, -2.0, epsilon); + EXPECT_NEAR(front_out_point.y, -10.0, epsilon); + + const auto back_out_point = s.getSplineInterpolatedPoint(4.0, 0.1); + EXPECT_NEAR(back_out_point.x, 10.0, epsilon); + EXPECT_NEAR(back_out_point.y, 12.5, epsilon); + + // out of range of index + EXPECT_THROW(s.getSplineInterpolatedPoint(-1, 0.0), std::out_of_range); + EXPECT_THROW(s.getSplineInterpolatedPoint(5, 0.0), std::out_of_range); + } + + { // yaw + // front + EXPECT_NEAR(s.getSplineInterpolatedYaw(0, 0.0), 1.368174, epsilon); + + // back + EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.0), 0.278594, epsilon); + + // random + 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.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); + EXPECT_THROW(s.getSplineInterpolatedYaw(5, 0.0), std::out_of_range); + } + + { // curvature + // front + EXPECT_NEAR(s.getSplineInterpolatedCurvature(0, 0.0), 0.0, epsilon); + + // back + EXPECT_NEAR(s.getSplineInterpolatedCurvature(4, 0.0), 0.0, epsilon); + + // random + EXPECT_NEAR(s.getSplineInterpolatedCurvature(3, 0.5), -0.271073, epsilon); + + // out of range of total length + EXPECT_NEAR(s.getSplineInterpolatedCurvature(0.0, -0.1), 0.0, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedCurvature(4, 0.1), 0.0, epsilon); + + // out of range of index + EXPECT_THROW(s.getSplineInterpolatedCurvature(-1, 0.0), std::out_of_range); + EXPECT_THROW(s.getSplineInterpolatedCurvature(5, 0.0), std::out_of_range); + } + + { // accumulated distance + // front + EXPECT_NEAR(s.getAccumulatedLength(0), 0.0, epsilon); + + // back + EXPECT_NEAR(s.getAccumulatedLength(4), 26.8488511, epsilon); + + // random + EXPECT_NEAR(s.getAccumulatedLength(3), 21.2586811, epsilon); + + // out of range of index + EXPECT_THROW(s.getAccumulatedLength(-1), std::out_of_range); + EXPECT_THROW(s.getAccumulatedLength(5), std::out_of_range); + } + + // size of base_keys is 1 (infeasible to interpolate) + std::vector single_points; + single_points.push_back(createPoint(1.0, 0.0, 0.0)); + EXPECT_THROW(SplineInterpolationPoints2d{single_points}, std::logic_error); +} + +TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism) +{ + using autoware::universe_utils::createPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; + + std::vector points; + points.push_back(createPoint(-2.0, -10.0, 0.0)); + points.push_back(createPoint(2.0, 1.5, 0.0)); + points.push_back(createPoint(3.0, 3.0, 0.0)); + + std::vector trajectory_points; + for (const auto & p : points) { + TrajectoryPoint tp; + tp.pose.position = p; + trajectory_points.push_back(tp); + } + + SplineInterpolationPoints2d s_point(points); + s_point.getSplineInterpolatedPoint(0, 0.); + + SplineInterpolationPoints2d s_traj_point(trajectory_points); + s_traj_point.getSplineInterpolatedPoint(0, 0.); +} diff --git a/common/autoware_interpolation/test/src/test_zero_order_hold.cpp b/common/autoware_interpolation/test/src/test_zero_order_hold.cpp new file mode 100644 index 0000000000..c99348d66d --- /dev/null +++ b/common/autoware_interpolation/test/src/test_zero_order_hold.cpp @@ -0,0 +1,158 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/interpolation/zero_order_hold.hpp" + +#include + +#include +#include + +constexpr double epsilon = 1e-6; + +TEST(zero_order_hold_interpolation, vector_interpolation) +{ + { // straight: query_keys is same as base_keys + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector base_values{0.0, 1.5, 2.5, 3.5, 0.0}; + const std::vector query_keys = base_keys; + const std::vector ans = base_values; + + 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); + } + } + + { // straight: query_keys is random + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector base_values{0.0, 1.5, 3.0, 4.5, 6.0}; + 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 = + 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); + } + } + + { // curve: query_keys is same as base_keys + 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 = base_keys; + const std::vector ans = base_values; + + 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); + } + } + + { // curve: query_keys is same as random + const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; + const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; + const std::vector query_keys{0.0, 8.0, 18.0}; + const std::vector ans{-1.2, 1.0, 2.0}; + + 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); + } + } + + { // Boundary Condition + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector base_values{0.0, 1.5, 2.5, 3.5, 0.0}; + 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 = + 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); + } + } + + { // Boundary Condition + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector base_values{0.0, 1.5, 2.5, 3.5, 0.0}; + 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 = + 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); + } + } +} + +TEST(zero_order_hold_interpolation, vector_interpolation_no_double_interpolation) +{ + { // straight: query_keys is same as base_keys + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector base_values{true, true, false, true, true}; + const std::vector query_keys = base_keys; + const auto ans = base_values; + + 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)); + } + } + + { // straight: query_keys is random + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector base_values{true, false, false, true, false}; + 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 = + 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)); + } + } + + { // Boundary Condition + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector base_values{true, true, false, true, false}; + 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 = + 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); + } + } + + { // Boundary Condition + const std::vector base_keys{0.0, 1.0, 2.0, 3.0, 4.0}; + const std::vector base_values{true, false, true, true, false}; + 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 = + 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); + } + } +} From 1b1f1775de0503d35289c31ba2b7ebc088e3faad Mon Sep 17 00:00:00 2001 From: NorahXiong <103234047+NorahXiong@users.noreply.github.com> Date: Wed, 15 Jan 2025 15:49:59 +0800 Subject: [PATCH 02/13] Update README.md: fix LaTeX syntax error. Signed-off-by: NorahXiong --- common/autoware_interpolation/README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/common/autoware_interpolation/README.md b/common/autoware_interpolation/README.md index 5801c06dcb..92d66a50d8 100644 --- a/common/autoware_interpolation/README.md +++ b/common/autoware_interpolation/README.md @@ -41,10 +41,10 @@ $$ \begin{align} Y_i (x_i) & = y_i \ \ \ (i = 0, \dots, N-1) \\ Y_i (x_{i+1}) & = y_{i+1} \ \ \ (i = 0, \dots, N-1) \\ -Y'_i (x_{i+1}) & = Y'_{i+1} (x_{i+1}) \ \ \ (i = 0, \dots, N-2) \\ -Y''_i (x_{i+1}) & = Y''_{i+1} (x_{i+1}) \ \ \ (i = 0, \dots, N-2) \\ -Y''_0 (x_0) & = 0 \\ -Y''_{N-1} (x_N) & = 0 +Y_i '(x_{i+1}) & = Y_{i+1}' (x_{i+1}) \ \ \ (i = 0, \dots, N-2) \\ +Y_i (x_{i+1})'' & = Y_{i+1}'' (x_{i+1}) \ \ \ (i = 0, \dots, N-2) \\ +Y_0 ''(x_0) & = 0 \\ +Y_{N-1}'' (x_N) & = 0 \end{align} $$ From 89fb2b03bae8c6abfdcfa9c8bf3967c16d10ee7d Mon Sep 17 00:00:00 2001 From: NorahXiong <103234047+NorahXiong@users.noreply.github.com> Date: Wed, 15 Jan 2025 16:00:06 +0800 Subject: [PATCH 03/13] Update common/autoware_interpolation/package.xml Co-authored-by: Yutaka Kondo Signed-off-by: NorahXiong --- common/autoware_interpolation/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/autoware_interpolation/package.xml b/common/autoware_interpolation/package.xml index 3bad757405..aa01d82a9a 100644 --- a/common/autoware_interpolation/package.xml +++ b/common/autoware_interpolation/package.xml @@ -2,7 +2,7 @@ autoware_interpolation - 0.40.0 + 0.0.0 The spline interpolation package Fumiya Watanabe Takayuki Murooka From de2d4cf50171a879bc32a12f551e276c2d8189e2 Mon Sep 17 00:00:00 2001 From: NorahXiong <103234047+NorahXiong@users.noreply.github.com> Date: Wed, 15 Jan 2025 16:00:35 +0800 Subject: [PATCH 04/13] Delete common/autoware_interpolation/CHANGELOG.rst Signed-off-by: NorahXiong --- common/autoware_interpolation/CHANGELOG.rst | 51 --------------------- 1 file changed, 51 deletions(-) delete mode 100644 common/autoware_interpolation/CHANGELOG.rst diff --git a/common/autoware_interpolation/CHANGELOG.rst b/common/autoware_interpolation/CHANGELOG.rst deleted file mode 100644 index 37fc48c525..0000000000 --- a/common/autoware_interpolation/CHANGELOG.rst +++ /dev/null @@ -1,51 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_interpolation -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - common (`#9564 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(autoware_interpolation): prefix package and namespace with autoware (`#8088 `_) - Co-authored-by: kosuke55 -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- From 10007dead2c7332a82e34bcb2065b12ea031b8f8 Mon Sep 17 00:00:00 2001 From: NorahXiong Date: Fri, 17 Jan 2025 09:35:37 +0800 Subject: [PATCH 05/13] fix(autoware_interpolation): adaption to autoware_utils Signed-off-by: NorahXiong --- .../interpolation/spline_interpolation.hpp | 2 +- .../spline_interpolation_points_2d.hpp | 2 +- common/autoware_interpolation/package.xml | 2 +- .../src/spline_interpolation_points_2d.cpp | 2 +- .../test/src/test_spline_interpolation.cpp | 2 +- .../test_spline_interpolation_points_2d.cpp | 58 +++++++++---------- 6 files changed, 34 insertions(+), 34 deletions(-) diff --git a/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp index f7feada78f..b6f5ae3e5a 100644 --- a/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_ #include "autoware/interpolation/interpolation_utils.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include diff --git a/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp index c9668a4cad..68d79cff0b 100644 --- a/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation_points_2d.hpp @@ -49,7 +49,7 @@ class SplineInterpolationPoints2d { std::vector points_inner; for (const auto & p : points) { - points_inner.push_back(autoware::universe_utils::getPoint(p)); + points_inner.push_back(autoware_utils::get_point(p)); } calcSplineCoefficientsInner(points_inner); } diff --git a/common/autoware_interpolation/package.xml b/common/autoware_interpolation/package.xml index aa01d82a9a..a007a4eb07 100644 --- a/common/autoware_interpolation/package.xml +++ b/common/autoware_interpolation/package.xml @@ -11,8 +11,8 @@ ament_cmake_auto autoware_cmake - autoware_universe_utils eigen + autoware_utils ament_cmake_ros ament_lint_auto diff --git a/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp b/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp index e0f55cfb24..5449fbdac8 100644 --- a/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp @@ -90,7 +90,7 @@ geometry_msgs::msg::Pose SplineInterpolationPoints2d::getSplineInterpolatedPose( geometry_msgs::msg::Pose pose; pose.position = getSplineInterpolatedPoint(idx, s); pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(getSplineInterpolatedYaw(idx, s)); + autoware_utils::create_quaternion_from_yaw(getSplineInterpolatedYaw(idx, s)); return pose; } diff --git a/common/autoware_interpolation/test/src/test_spline_interpolation.cpp b/common/autoware_interpolation/test/src/test_spline_interpolation.cpp index 766e94a47b..f80c122593 100644 --- a/common/autoware_interpolation/test/src/test_spline_interpolation.cpp +++ b/common/autoware_interpolation/test/src/test_spline_interpolation.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include "autoware/interpolation/spline_interpolation.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include 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 974ad606b9..a55b7f5d57 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 @@ -14,7 +14,7 @@ #include "autoware/interpolation/spline_interpolation.hpp" #include "autoware/interpolation/spline_interpolation_points_2d.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" #include @@ -27,15 +27,15 @@ using autoware::interpolation::SplineInterpolationPoints2d; TEST(spline_interpolation, splineYawFromPoints) { - using autoware::universe_utils::createPoint; + using autoware_utils::create_point; { // straight std::vector points; - points.push_back(createPoint(0.0, 0.0, 0.0)); - points.push_back(createPoint(1.0, 1.5, 0.0)); - points.push_back(createPoint(2.0, 3.0, 0.0)); - points.push_back(createPoint(3.0, 4.5, 0.0)); - points.push_back(createPoint(4.0, 6.0, 0.0)); + points.push_back(create_point(0.0, 0.0, 0.0)); + points.push_back(create_point(1.0, 1.5, 0.0)); + points.push_back(create_point(2.0, 3.0, 0.0)); + points.push_back(create_point(3.0, 4.5, 0.0)); + points.push_back(create_point(4.0, 6.0, 0.0)); const std::vector ans{0.9827937, 0.9827937, 0.9827937, 0.9827937, 0.9827937}; @@ -47,11 +47,11 @@ TEST(spline_interpolation, splineYawFromPoints) { // curve std::vector points; - points.push_back(createPoint(-2.0, -10.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - points.push_back(createPoint(3.0, 3.0, 0.0)); - points.push_back(createPoint(5.0, 10.0, 0.0)); - points.push_back(createPoint(10.0, 12.5, 0.0)); + points.push_back(create_point(-2.0, -10.0, 0.0)); + points.push_back(create_point(2.0, 1.5, 0.0)); + points.push_back(create_point(3.0, 3.0, 0.0)); + points.push_back(create_point(5.0, 10.0, 0.0)); + points.push_back(create_point(10.0, 12.5, 0.0)); const std::vector ans{1.368174, 0.961318, 1.086098, 0.938357, 0.278594}; const auto yaws = autoware::interpolation::splineYawFromPoints(points); @@ -62,15 +62,15 @@ TEST(spline_interpolation, splineYawFromPoints) { // size of base_keys is 1 (infeasible to interpolate) std::vector points; - points.push_back(createPoint(1.0, 0.0, 0.0)); + points.push_back(create_point(1.0, 0.0, 0.0)); EXPECT_THROW(autoware::interpolation::splineYawFromPoints(points), std::logic_error); } { // straight: size of base_keys is 2 (edge case in the implementation) std::vector points; - points.push_back(createPoint(1.0, 0.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); + points.push_back(create_point(1.0, 0.0, 0.0)); + points.push_back(create_point(2.0, 1.5, 0.0)); const std::vector ans{0.9827937, 0.9827937}; @@ -82,9 +82,9 @@ TEST(spline_interpolation, splineYawFromPoints) { // straight: size of base_keys is 3 (edge case in the implementation) std::vector points; - points.push_back(createPoint(1.0, 0.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - points.push_back(createPoint(3.0, 3.0, 0.0)); + points.push_back(create_point(1.0, 0.0, 0.0)); + points.push_back(create_point(2.0, 1.5, 0.0)); + points.push_back(create_point(3.0, 3.0, 0.0)); const std::vector ans{0.9827937, 0.9827937, 0.9827937}; @@ -97,15 +97,15 @@ TEST(spline_interpolation, splineYawFromPoints) TEST(spline_interpolation, SplineInterpolationPoints2d) { - using autoware::universe_utils::createPoint; + using autoware_utils::create_point; // curve std::vector points; - points.push_back(createPoint(-2.0, -10.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - points.push_back(createPoint(3.0, 3.0, 0.0)); - points.push_back(createPoint(5.0, 10.0, 0.0)); - points.push_back(createPoint(10.0, 12.5, 0.0)); + points.push_back(create_point(-2.0, -10.0, 0.0)); + points.push_back(create_point(2.0, 1.5, 0.0)); + points.push_back(create_point(3.0, 3.0, 0.0)); + points.push_back(create_point(5.0, 10.0, 0.0)); + points.push_back(create_point(10.0, 12.5, 0.0)); SplineInterpolationPoints2d s(points); @@ -194,19 +194,19 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) // size of base_keys is 1 (infeasible to interpolate) std::vector single_points; - single_points.push_back(createPoint(1.0, 0.0, 0.0)); + single_points.push_back(create_point(1.0, 0.0, 0.0)); EXPECT_THROW(SplineInterpolationPoints2d{single_points}, std::logic_error); } TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism) { - using autoware::universe_utils::createPoint; + using autoware_utils::create_point; using autoware_planning_msgs::msg::TrajectoryPoint; std::vector points; - points.push_back(createPoint(-2.0, -10.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - points.push_back(createPoint(3.0, 3.0, 0.0)); + points.push_back(create_point(-2.0, -10.0, 0.0)); + points.push_back(create_point(2.0, 1.5, 0.0)); + points.push_back(create_point(3.0, 3.0, 0.0)); std::vector trajectory_points; for (const auto & p : points) { From 380bf1d7ea623f1eeb8b78559537011df2378d6a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 17 Jan 2025 01:36:57 +0000 Subject: [PATCH 06/13] style(pre-commit): autofix Signed-off-by: NorahXiong --- common/autoware_interpolation/package.xml | 2 +- .../src/spline_interpolation_points_2d.cpp | 3 +-- .../test/src/test_spline_interpolation_points_2d.cpp | 2 +- 3 files changed, 3 insertions(+), 4 deletions(-) diff --git a/common/autoware_interpolation/package.xml b/common/autoware_interpolation/package.xml index a007a4eb07..92dfe0b3d3 100644 --- a/common/autoware_interpolation/package.xml +++ b/common/autoware_interpolation/package.xml @@ -11,8 +11,8 @@ ament_cmake_auto autoware_cmake - eigen autoware_utils + eigen ament_cmake_ros ament_lint_auto diff --git a/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp b/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp index 5449fbdac8..67937a1a52 100644 --- a/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp @@ -89,8 +89,7 @@ geometry_msgs::msg::Pose SplineInterpolationPoints2d::getSplineInterpolatedPose( { geometry_msgs::msg::Pose pose; pose.position = getSplineInterpolatedPoint(idx, s); - pose.orientation = - autoware_utils::create_quaternion_from_yaw(getSplineInterpolatedYaw(idx, s)); + pose.orientation = autoware_utils::create_quaternion_from_yaw(getSplineInterpolatedYaw(idx, s)); return pose; } 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 a55b7f5d57..0af6be21a0 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 @@ -200,8 +200,8 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism) { - using autoware_utils::create_point; using autoware_planning_msgs::msg::TrajectoryPoint; + using autoware_utils::create_point; std::vector points; points.push_back(create_point(-2.0, -10.0, 0.0)); From 371acaca9617efbc64ab0c760fb39ab59a38a0de Mon Sep 17 00:00:00 2001 From: NorahXiong Date: Fri, 24 Jan 2025 10:47:42 +0800 Subject: [PATCH 07/13] feat(build_depends.repos): update dependent repositories Signed-off-by: NorahXiong --- build_depends.repos | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/build_depends.repos b/build_depends.repos index 0b81f20483..b83e73941c 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -3,3 +3,11 @@ repositories: type: git url: https://github.com/autowarefoundation/autoware_msgs.git version: main + autoware_utils: + type: git + url: https://github.com/autowarefoundation/autoware_utils.git + version: main + autoware_internal_msgs/autoware_internal_msgs: + type: git + url: https://github.com/autowarefoundation/autoware_internal_msgs.git + version: main \ No newline at end of file From f81eb224809d6db9e9bcc53d61cf0c8e7079e2ec Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 24 Jan 2025 03:00:16 +0000 Subject: [PATCH 08/13] style(pre-commit): autofix Signed-off-by: NorahXiong --- build_depends.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build_depends.repos b/build_depends.repos index b83e73941c..9e708f6664 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -10,4 +10,4 @@ repositories: autoware_internal_msgs/autoware_internal_msgs: type: git url: https://github.com/autowarefoundation/autoware_internal_msgs.git - version: main \ No newline at end of file + version: main From 772d2381f1a5c7b25fe5d0d4383affa4538fe0d2 Mon Sep 17 00:00:00 2001 From: NorahXiong <103234047+NorahXiong@users.noreply.github.com> Date: Fri, 24 Jan 2025 11:39:35 +0800 Subject: [PATCH 09/13] Update build_depends.repos Co-authored-by: Yutaka Kondo --- build_depends.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build_depends.repos b/build_depends.repos index 9e708f6664..6dae4778b4 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -7,7 +7,7 @@ repositories: type: git url: https://github.com/autowarefoundation/autoware_utils.git version: main - autoware_internal_msgs/autoware_internal_msgs: + autoware_internal_msgs: type: git url: https://github.com/autowarefoundation/autoware_internal_msgs.git version: main From f4db7fe65c60c08e6805c4f840f71c5c15772f04 Mon Sep 17 00:00:00 2001 From: NorahXiong <103234047+NorahXiong@users.noreply.github.com> Date: Fri, 24 Jan 2025 11:39:50 +0800 Subject: [PATCH 10/13] Update common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp Co-authored-by: Yutaka Kondo --- .../include/autoware/interpolation/spline_interpolation.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp index b6f5ae3e5a..f4344b0fc4 100644 --- a/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp @@ -16,7 +16,7 @@ #define AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_ #include "autoware/interpolation/interpolation_utils.hpp" -#include "autoware_utils/geometry/geometry.hpp" +#include #include From 3b511cc8f3121811e3f9134c5df224efbe4b3fca Mon Sep 17 00:00:00 2001 From: NorahXiong <103234047+NorahXiong@users.noreply.github.com> Date: Fri, 24 Jan 2025 11:39:56 +0800 Subject: [PATCH 11/13] Update common/autoware_interpolation/test/src/test_spline_interpolation.cpp Co-authored-by: Yutaka Kondo --- .../test/src/test_spline_interpolation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/autoware_interpolation/test/src/test_spline_interpolation.cpp b/common/autoware_interpolation/test/src/test_spline_interpolation.cpp index f80c122593..e37d33e167 100644 --- a/common/autoware_interpolation/test/src/test_spline_interpolation.cpp +++ b/common/autoware_interpolation/test/src/test_spline_interpolation.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include "autoware/interpolation/spline_interpolation.hpp" -#include "autoware_utils/geometry/geometry.hpp" +#include #include From 0007f90b06789334639031cbbadafe6b6e140e07 Mon Sep 17 00:00:00 2001 From: NorahXiong <103234047+NorahXiong@users.noreply.github.com> Date: Fri, 24 Jan 2025 11:40:03 +0800 Subject: [PATCH 12/13] Update common/autoware_interpolation/test/src/test_spline_interpolation_points_2d.cpp Co-authored-by: Yutaka Kondo --- .../test/src/test_spline_interpolation_points_2d.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 0af6be21a0..f14385f044 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 @@ -14,7 +14,7 @@ #include "autoware/interpolation/spline_interpolation.hpp" #include "autoware/interpolation/spline_interpolation_points_2d.hpp" -#include "autoware_utils/geometry/geometry.hpp" +#include #include From 40477de65bec99e1a37dcd14f63f83e68e280eac Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 24 Jan 2025 03:40:31 +0000 Subject: [PATCH 13/13] style(pre-commit): autofix --- .../include/autoware/interpolation/spline_interpolation.hpp | 2 +- .../test/src/test_spline_interpolation.cpp | 1 + .../test/src/test_spline_interpolation_points_2d.cpp | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp index f4344b0fc4..f3b4b525d0 100644 --- a/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp +++ b/common/autoware_interpolation/include/autoware/interpolation/spline_interpolation.hpp @@ -16,9 +16,9 @@ #define AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_ #include "autoware/interpolation/interpolation_utils.hpp" -#include #include +#include #include #include diff --git a/common/autoware_interpolation/test/src/test_spline_interpolation.cpp b/common/autoware_interpolation/test/src/test_spline_interpolation.cpp index e37d33e167..ea99b61d91 100644 --- a/common/autoware_interpolation/test/src/test_spline_interpolation.cpp +++ b/common/autoware_interpolation/test/src/test_spline_interpolation.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "autoware/interpolation/spline_interpolation.hpp" + #include #include 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 f14385f044..4c816c3893 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 @@ -14,6 +14,7 @@ #include "autoware/interpolation/spline_interpolation.hpp" #include "autoware/interpolation/spline_interpolation_points_2d.hpp" + #include #include