diff --git a/common/autoware_interpolation/CMakeLists.txt b/common/autoware_interpolation/CMakeLists.txt new file mode 100644 index 00000000..09797272 --- /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 00000000..92d66a50 --- /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 00000000..1c0913c8 --- /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 00000000..762806b3 --- /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 00000000..2e4a8fbd --- /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 00000000..b6f5ae3e --- /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_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 00000000..68d79cff --- /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_utils::get_point(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 00000000..c28f8708 --- /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 00000000..92dfe0b3 --- /dev/null +++ b/common/autoware_interpolation/package.xml @@ -0,0 +1,24 @@ + + + + autoware_interpolation + 0.0.0 + The spline interpolation package + Fumiya Watanabe + Takayuki Murooka + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_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 00000000..4ba339f0 --- /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 00000000..5acdf6a9 --- /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 00000000..78f3778a --- /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 00000000..67937a1a --- /dev/null +++ b/common/autoware_interpolation/src/spline_interpolation_points_2d.cpp @@ -0,0 +1,211 @@ +// 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_utils::create_quaternion_from_yaw(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 00000000..ebb93a0a --- /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 00000000..9131b458 --- /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 00000000..0fea1e51 --- /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 00000000..b7ce134c --- /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 00000000..f80c1225 --- /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_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 00000000..0af6be21 --- /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_utils/geometry/geometry.hpp" + +#include + +#include +#include + +constexpr double epsilon = 1e-6; + +using autoware::interpolation::SplineInterpolationPoints2d; + +TEST(spline_interpolation, splineYawFromPoints) +{ + using autoware_utils::create_point; + + { // straight + std::vector points; + 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}; + + 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(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); + 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(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(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}; + + 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(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}; + + 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_utils::create_point; + + // curve + std::vector points; + 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); + + { // 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(create_point(1.0, 0.0, 0.0)); + EXPECT_THROW(SplineInterpolationPoints2d{single_points}, std::logic_error); +} + +TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism) +{ + 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)); + 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) { + 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 00000000..c99348d6 --- /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); + } + } +} diff --git a/common/autoware_motion_utils/CMakeLists.txt b/common/autoware_motion_utils/CMakeLists.txt new file mode 100644 index 00000000..4c36ef2f --- /dev/null +++ b/common/autoware_motion_utils/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_motion_utils) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Boost REQUIRED) + +ament_auto_add_library(autoware_motion_utils SHARED + DIRECTORY src +) + +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) + + file(GLOB_RECURSE test_files test/**/*.cpp) + + ament_add_ros_isolated_gtest(test_autoware_motion_utils ${test_files}) + + target_link_libraries(test_autoware_motion_utils + autoware_motion_utils + ) +endif() + +ament_auto_package() diff --git a/common/autoware_motion_utils/README.md b/common/autoware_motion_utils/README.md new file mode 100644 index 00000000..8d80ae92 --- /dev/null +++ b/common/autoware_motion_utils/README.md @@ -0,0 +1,104 @@ +# Motion Utils package + +## Definition of terms + +### Segment + +`Segment` in Autoware is the line segment between two successive points as follows. + +![segment](./media/segment.svg){: style="width:600px"} + +The nearest segment index and nearest point index to a certain position is not always th same. +Therefore, we prepare two different utility functions to calculate a nearest index for points and segments. + +## Nearest index search + +In this section, the nearest index and nearest segment index search is explained. + +We have the same functions for the nearest index search and nearest segment index search. +Taking for the example the nearest index search, we have two types of functions. + +The first function finds the nearest index with distance and yaw thresholds. + +```cpp +template +size_t findFirstNearestIndexWithSoftConstraints( + const T & points, const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); +``` + +This function finds the first local solution within thresholds. +The reason to find the first local one is to deal with some edge cases explained in the next subsection. + +There are default parameters for thresholds arguments so that you can decide which thresholds to pass to the function. + +1. When both the distance and yaw thresholds are given. + - First, try to find the nearest index with both the distance and yaw thresholds. + - If not found, try to find again with only the distance threshold. + - If not found, find without any thresholds. +2. When only distance are given. + - First, try to find the nearest index the distance threshold. + - If not found, find without any thresholds. +3. When no thresholds are given. + - Find the nearest index. + +The second function finds the nearest index in the lane whose id is `lane_id`. + +```cpp +size_t findNearestIndexFromLaneId( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & pos, const int64_t lane_id); +``` + +### Application to various object + +Many node packages often calculate the nearest index of objects. +We will explain the recommended method to calculate it. + +#### Nearest index for the ego + +Assuming that the path length before the ego is short enough, we expect to find the correct nearest index in the following edge cases by `findFirstNearestIndexWithSoftConstraints` with both distance and yaw thresholds. +Blue circles describes the distance threshold from the base link position and two blue lines describe the yaw threshold against the base link orientation. +Among points in these cases, the correct nearest point which is red can be found. + +![ego_nearest_search](./media/ego_nearest_search.svg) + +Therefore, the implementation is as follows. + +```cpp +const size_t ego_nearest_idx = findFirstNearestIndexWithSoftConstraints(points, ego_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); +const size_t ego_nearest_seg_idx = findFirstNearestIndexWithSoftConstraints(points, ego_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); +``` + +#### Nearest index for dynamic objects + +For the ego nearest index, the orientation is considered in addition to the position since the ego is supposed to follow the points. +However, for the dynamic objects (e.g., predicted object), sometimes its orientation may be different from the points order, e.g. the dynamic object driving backward although the ego is driving forward. + +Therefore, the yaw threshold should not be considered for the dynamic object. +The implementation is as follows. + +```cpp +const size_t dynamic_obj_nearest_idx = findFirstNearestIndexWithSoftConstraints(points, dynamic_obj_pose, dynamic_obj_nearest_dist_threshold); +const size_t dynamic_obj_nearest_seg_idx = findFirstNearestIndexWithSoftConstraints(points, dynamic_obj_pose, dynamic_obj_nearest_dist_threshold); +``` + +#### Nearest index for traffic objects + +In lanelet maps, traffic objects belong to the specific lane. +With this specific lane's id, the correct nearest index can be found. + +The implementation is as follows. + +```cpp +// first extract `lane_id` which the traffic object belong to. +const size_t traffic_obj_nearest_idx = findNearestIndexFromLaneId(path_with_lane_id, traffic_obj_pos, lane_id); +const size_t traffic_obj_nearest_seg_idx = findNearestSegmentIndexFromLaneId(path_with_lane_id, traffic_obj_pos, lane_id); +``` + +## For developers + +Some of the template functions in `trajectory.hpp` are mostly used for specific types (`autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::TrajectoryPoint`), so they are exported as `extern template` functions to speed-up compilation time. + +`autoware_motion_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. diff --git a/common/autoware_motion_utils/docs/vehicle/vehicle.md b/common/autoware_motion_utils/docs/vehicle/vehicle.md new file mode 100644 index 00000000..734139aa --- /dev/null +++ b/common/autoware_motion_utils/docs/vehicle/vehicle.md @@ -0,0 +1,169 @@ +# vehicle utils + +Vehicle utils provides a convenient library used to check vehicle status. + +## Feature + +The library contains following classes. + +### vehicle_stop_checker + +This class check whether the vehicle is stopped or not based on localization result. + +#### Subscribed Topics + +| Name | Type | Description | +| ------------------------------- | ------------------------- | ---------------- | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | vehicle odometry | + +#### Parameters + +| Name | Type | Default Value | Explanation | +| -------------------------- | ------ | ------------- | --------------------------- | +| `velocity_buffer_time_sec` | double | 10.0 | odometry buffering time [s] | + +#### Member functions + +```c++ +bool isVehicleStopped(const double stop_duration) +``` + +- Check simply whether the vehicle is stopped based on the localization result. +- Returns `true` if the vehicle is stopped, even if system outputs a non-zero target velocity. + +#### Example Usage + +Necessary includes: + +```c++ +#include +``` + +1.Create a checker instance. + +```c++ +class SampleNode : public rclcpp::Node +{ +public: + SampleNode() : Node("sample_node") + { + vehicle_stop_checker_ = std::make_unique(this); + } + + std::unique_ptr vehicle_stop_checker_; + + bool sampleFunc(); + + ... +} +``` + +2.Check the vehicle state. + +```c++ + +bool SampleNode::sampleFunc() +{ + ... + + const auto result_1 = vehicle_stop_checker_->isVehicleStopped(); + + ... + + const auto result_2 = vehicle_stop_checker_->isVehicleStopped(3.0); + + ... +} + +``` + +### vehicle_arrival_checker + +This class check whether the vehicle arrive at stop point based on localization and planning result. + +#### Subscribed Topics + +| Name | Type | Description | +| ---------------------------------------- | ----------------------------------------- | ---------------- | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | vehicle odometry | +| `/planning/scenario_planning/trajectory` | `autoware_planning_msgs::msg::Trajectory` | trajectory | + +#### Parameters + +| Name | Type | Default Value | Explanation | +| -------------------------- | ------ | ------------- | ---------------------------------------------------------------------- | +| `velocity_buffer_time_sec` | double | 10.0 | odometry buffering time [s] | +| `th_arrived_distance_m` | double | 1.0 | threshold distance to check if vehicle has arrived at target point [m] | + +#### Member functions + +```c++ +bool isVehicleStopped(const double stop_duration) +``` + +- Check simply whether the vehicle is stopped based on the localization result. +- Returns `true` if the vehicle is stopped, even if system outputs a non-zero target velocity. + +```c++ +bool isVehicleStoppedAtStopPoint(const double stop_duration) +``` + +- Check whether the vehicle is stopped at stop point based on the localization and planning result. +- Returns `true` if the vehicle is not only stopped but also arrived at stop point. + +#### Example Usage + +Necessary includes: + +```c++ +#include +``` + +1.Create a checker instance. + +```c++ +class SampleNode : public rclcpp::Node +{ +public: + SampleNode() : Node("sample_node") + { + vehicle_arrival_checker_ = std::make_unique(this); + } + + std::unique_ptr vehicle_arrival_checker_; + + bool sampleFunc(); + + ... +} +``` + +2.Check the vehicle state. + +```c++ + +bool SampleNode::sampleFunc() +{ + ... + + const auto result_1 = vehicle_arrival_checker_->isVehicleStopped(); + + ... + + const auto result_2 = vehicle_arrival_checker_->isVehicleStopped(3.0); + + ... + + const auto result_3 = vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(); + + ... + + const auto result_4 = vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(3.0); + + ... +} +``` + +## Assumptions / Known limits + +`vehicle_stop_checker` and `vehicle_arrival_checker` cannot check whether the vehicle is stopped more than `velocity_buffer_time_sec` second. diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp new file mode 100644 index 00000000..9c1f7c87 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/constants.hpp @@ -0,0 +1,23 @@ +// 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__MOTION_UTILS__CONSTANTS_HPP_ +#define AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_ + +namespace autoware::motion_utils +{ +constexpr double overlap_threshold = 0.1; +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp new file mode 100644 index 00000000..30852895 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp @@ -0,0 +1,33 @@ +// 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. + +#ifndef AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_ +#define AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_ + +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_utils +{ +std::optional calcDecelDistWithJerkAndAccConstraints( + const double current_vel, const double target_vel, const double current_acc, const double acc_min, + const double jerk_acc, const double jerk_dec); + +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/factor/planning_factor_interface.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/factor/planning_factor_interface.hpp new file mode 100644 index 00000000..9772349a --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/factor/planning_factor_interface.hpp @@ -0,0 +1,242 @@ +// Copyright 2024 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__MOTION_UTILS__FACTOR__PLANNING_FACTOR_INTERFACE_HPP_ +#define AUTOWARE__MOTION_UTILS__FACTOR__PLANNING_FACTOR_INTERFACE_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace autoware::motion_utils +{ + +using autoware_internal_planning_msgs::msg::ControlPoint; +using autoware_internal_planning_msgs::msg::PlanningFactor; +using autoware_internal_planning_msgs::msg::PlanningFactorArray; +using autoware_internal_planning_msgs::msg::SafetyFactorArray; +using geometry_msgs::msg::Pose; + +class PlanningFactorInterface +{ +public: + PlanningFactorInterface(rclcpp::Node * node, const std::string & name) + : name_{name}, + pub_factors_{ + node->create_publisher("/planning/planning_factors/" + name, 1)}, + clock_{node->get_clock()} + { + } + + /** + * @brief factor setter for single control point. + * + * @param path points. + * @param ego current pose. + * @param control point pose. (e.g. stop or slow down point pose) + * @param behavior of this planning factor. + * @param safety factor. + * @param driving direction. + * @param target velocity of the control point. + * @param shift length of the control point. + * @param detail information. + */ + template + void add( + const std::vector & points, const Pose & ego_pose, const Pose & control_point_pose, + const uint16_t behavior, const SafetyFactorArray & safety_factors, + const bool is_driving_forward = true, const double velocity = 0.0, + const double shift_length = 0.0, const std::string & detail = "") + { + const auto distance = static_cast(autoware::motion_utils::calcSignedArcLength( + points, ego_pose.position, control_point_pose.position)); + add( + distance, control_point_pose, behavior, safety_factors, is_driving_forward, velocity, + shift_length, detail); + } + + /** + * @brief factor setter for two control points (section). + * + * @param path points. + * @param ego current pose. + * @param control section start pose. (e.g. lane change start point pose) + * @param control section end pose. (e.g. lane change end point pose) + * @param behavior of this planning factor. + * @param safety factor. + * @param driving direction. + * @param target velocity of the control point. + * @param shift length of the control point. + * @param detail information. + */ + template + void add( + const std::vector & points, const Pose & ego_pose, const Pose & start_pose, + const Pose & end_pose, const uint16_t behavior, const SafetyFactorArray & safety_factors, + const bool is_driving_forward = true, const double velocity = 0.0, + const double shift_length = 0.0, const std::string & detail = "") + { + const auto start_distance = static_cast( + autoware::motion_utils::calcSignedArcLength(points, ego_pose.position, start_pose.position)); + const auto end_distance = static_cast( + autoware::motion_utils::calcSignedArcLength(points, ego_pose.position, end_pose.position)); + add( + start_distance, end_distance, start_pose, end_pose, behavior, safety_factors, + is_driving_forward, velocity, shift_length, detail); + } + + /** + * @brief factor setter for single control point. + * + * @param distance to control point. + * @param control point pose. (e.g. stop point pose) + * @param behavior of this planning factor. + * @param safety factor. + * @param driving direction. + * @param target velocity of the control point. + * @param shift length of the control point. + * @param detail information. + */ + void add( + const double distance, const Pose & control_point_pose, const uint16_t behavior, + const SafetyFactorArray & safety_factors, const bool is_driving_forward = true, + const double velocity = 0.0, const double shift_length = 0.0, const std::string & detail = "") + { + const auto control_point = autoware_internal_planning_msgs::build() + .pose(control_point_pose) + .velocity(velocity) + .shift_length(shift_length) + .distance(distance); + + const auto factor = autoware_internal_planning_msgs::build() + .module(name_) + .is_driving_forward(is_driving_forward) + .control_points({control_point}) + .behavior(behavior) + .detail(detail) + .safety_factors(safety_factors); + + factors_.push_back(factor); + } + + /** + * @brief factor setter for two control points (section). + * + * @param distance to control section start point. + * @param distance to control section end point. + * @param control section start pose. (e.g. lane change start point pose) + * @param control section end pose. (e.g. lane change end point pose) + * @param behavior of this planning factor. + * @param safety factor. + * @param driving direction. + * @param target velocity of the control point. + * @param shift length of the control point. + * @param detail information. + */ + void add( + const double start_distance, const double end_distance, const Pose & start_pose, + const Pose & end_pose, const uint16_t behavior, const SafetyFactorArray & safety_factors, + const bool is_driving_forward = true, const double velocity = 0.0, + const double shift_length = 0.0, const std::string & detail = "") + { + const auto control_start_point = autoware_internal_planning_msgs::build() + .pose(start_pose) + .velocity(velocity) + .shift_length(shift_length) + .distance(start_distance); + + const auto control_end_point = autoware_internal_planning_msgs::build() + .pose(end_pose) + .velocity(velocity) + .shift_length(shift_length) + .distance(end_distance); + + const auto factor = autoware_internal_planning_msgs::build() + .module(name_) + .is_driving_forward(is_driving_forward) + .control_points({control_start_point, control_end_point}) + .behavior(behavior) + .detail(detail) + .safety_factors(safety_factors); + + factors_.push_back(factor); + } + + /** + * @brief publish planning factors. + */ + void publish() + { + PlanningFactorArray msg; + msg.header.frame_id = "map"; + msg.header.stamp = clock_->now(); + msg.factors = factors_; + + pub_factors_->publish(msg); + + factors_.clear(); + } + +private: + std::string name_; + + rclcpp::Publisher::SharedPtr pub_factors_; + + rclcpp::Clock::SharedPtr clock_; + + std::vector factors_; +}; + +extern template void +PlanningFactorInterface::add( + const std::vector &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, + const std::string &); +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, + const std::string &); + +extern template void +PlanningFactorInterface::add( + const std::vector &, const Pose &, + const Pose &, const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, + const double, const double, const std::string &); +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); + +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__FACTOR__PLANNING_FACTOR_INTERFACE_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/factor/steering_factor_interface.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/factor/steering_factor_interface.hpp new file mode 100644 index 00000000..5b701a73 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/factor/steering_factor_interface.hpp @@ -0,0 +1,51 @@ +// 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__MOTION_UTILS__FACTOR__STEERING_FACTOR_INTERFACE_HPP_ +#define AUTOWARE__MOTION_UTILS__FACTOR__STEERING_FACTOR_INTERFACE_HPP_ + +#include +#include +#include + +#include + +namespace autoware::motion_utils +{ + +using autoware_adapi_v1_msgs::msg::PlanningBehavior; +using autoware_adapi_v1_msgs::msg::SteeringFactor; +using SteeringFactorBehavior = SteeringFactor::_behavior_type; +using SteeringFactorStatus = SteeringFactor::_status_type; +using geometry_msgs::msg::Pose; + +class SteeringFactorInterface +{ +public: + [[nodiscard]] SteeringFactor get() const { return steering_factor_; } + void init(const SteeringFactorBehavior & behavior) { behavior_ = behavior; } + void reset() { steering_factor_.behavior = PlanningBehavior::UNKNOWN; } + + void set( + const std::array & pose, const std::array distance, + const uint16_t direction, const uint16_t status, const std::string & detail = ""); + +private: + SteeringFactorBehavior behavior_{SteeringFactor::UNKNOWN}; + SteeringFactor steering_factor_{}; +}; + +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__FACTOR__STEERING_FACTOR_INTERFACE_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp new file mode 100644 index 00000000..2fcde52e --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp @@ -0,0 +1,57 @@ + +// Copyright 2022-2024 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__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ +#define AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ + +#include +#include +#include +#include + +#include +#include + +namespace autoware::motion_utils +{ +using autoware_adapi_v1_msgs::msg::PlanningBehavior; +using autoware_adapi_v1_msgs::msg::VelocityFactor; +using VelocityFactorBehavior = VelocityFactor::_behavior_type; +using VelocityFactorStatus = VelocityFactor::_status_type; +using geometry_msgs::msg::Pose; + +class VelocityFactorInterface +{ +public: + [[nodiscard]] VelocityFactor get() const { return velocity_factor_; } + void init(const VelocityFactorBehavior & behavior) { behavior_ = behavior; } + void reset() { velocity_factor_.behavior = PlanningBehavior::UNKNOWN; } + + template + void set( + const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, + const VelocityFactorStatus status, const std::string & detail = ""); + + void set( + const double & distance, const VelocityFactorStatus & status, const std::string & detail = ""); + +private: + VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN}; + VelocityFactor velocity_factor_{}; +}; + +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp new file mode 100644 index 00000000..2b89fc19 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp @@ -0,0 +1,55 @@ +// 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__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ +#define AUTOWARE__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ + +#include + +#include + +#include + +namespace autoware::motion_utils +{ +using geometry_msgs::msg::Pose; + +visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( + const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); + +visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( + const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); + +visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( + const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); + +visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward); + +visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( + const rclcpp::Time & now, const int32_t id); + +visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker( + const rclcpp::Time & now, const int32_t id); +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp new file mode 100644 index 00000000..fd86c54d --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp @@ -0,0 +1,82 @@ +// 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. + +#ifndef AUTOWARE__MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ +#define AUTOWARE__MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ + +#include + +#include +#include + +#include +#include +#include +#include + +namespace autoware::motion_utils +{ + +/// @brief type of virtual wall associated with different marker styles and namespace +enum VirtualWallType { stop, slowdown, deadline, pass }; +/// @brief virtual wall to be visualized in rviz +struct VirtualWall +{ + geometry_msgs::msg::Pose pose{}; + std::string text{}; + std::string detail{}; + std::string ns{}; + VirtualWallType style = stop; + double longitudinal_offset{}; + bool is_driving_forward{true}; +}; +using VirtualWalls = std::vector; + +/// @brief class to manage the creation of virtual wall markers +/// @details creates both ADD and DELETE markers +class VirtualWallMarkerCreator +{ + struct MarkerCount + { + size_t previous = 0UL; + size_t current = 0UL; + }; + + using create_wall_function = std::function; + + VirtualWalls virtual_walls_; + std::unordered_map marker_count_per_namespace_; + + /// @brief internal cleanup: clear the stored markers and remove unused namespace from the map + void cleanup(); + +public: + /// @brief add a virtual wall + /// @param virtual_wall virtual wall to add + void add_virtual_wall(const VirtualWall & virtual_wall); + /// @brief add virtual walls + /// @param virtual_walls virtual walls to add + void add_virtual_walls(const VirtualWalls & walls); + + /// @brief create markers for the stored virtual walls + /// @details also create DELETE markers for the namespace+ids that are no longer used + /// @param now current time to be used for displaying the markers + visualization_msgs::msg::MarkerArray create_markers(const rclcpp::Time & now = rclcpp::Time()); +}; +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp new file mode 100644 index 00000000..9f8e214e --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample.hpp @@ -0,0 +1,240 @@ +// 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__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ +#define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ + +#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" + +#include + +namespace autoware::motion_utils +{ +/** + * @brief A resampling function for a path(points). Note that in a default setting, position xy are + * resampled by spline interpolation, position z are resampled by linear interpolation, and + * orientation of the resampled path are calculated by a forward difference method + * based on the interpolated position x and y. + * @param input_path input path(point) to resample + * @param resampled_arclength arclength that contains length of each resampling points from initial + * point + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @return resampled path(poses) + */ +std::vector resamplePointVector( + const std::vector & points, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, + const bool use_lerp_for_z = true); + +/** + * @brief A resampling function for a path(position). Note that in a default setting, position xy + * are resampled by spline interpolation, position z are resampled by linear interpolation, and + * orientation of the resampled path are calculated by a forward difference method + * based on the interpolated position x and y. + * @param input_path input path(position) to resample + * @param resample_interval resampling interval + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @return resampled path(poses) + */ +std::vector resamplePointVector( + const std::vector & points, const double resample_interval, + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true); + +/** + * @brief A resampling function for a path(poses). Note that in a default setting, position xy are + * resampled by spline interpolation, position z are resampled by linear interpolation, and + * orientation of the resampled path are calculated by a forward difference method + * based on the interpolated position x and y. + * @param input_path input path(poses) to resample + * @param resampled_arclength arclength that contains length of each resampling points from initial + * point + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @return resampled path(poses) + */ +std::vector resamplePoseVector( + const std::vector & points, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, + const bool use_lerp_for_z = true); + +/** + * @brief A resampling function for a path(poses). Note that in a default setting, position xy are + * resampled by spline interpolation, position z are resampled by linear interpolation, and + * orientation of the resampled path are calculated by a forward difference method + * based on the interpolated position x and y. + * @param input_path input path(poses) to resample + * @param resample_interval resampling interval + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @return resampled path(poses) + */ +std::vector resamplePoseVector( + const std::vector & points, const double resample_interval, + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true); + +/** + * @brief A resampling function for a path with lane id. Note that in a default setting, position xy + * are resampled by spline interpolation, position z are resampled by linear interpolation, + * longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is + * resampled by linear interpolation. Orientation of the resampled path are calculated by a + * forward difference method based on the interpolated position x and y. Moreover, lane_ids + * and is_final are also interpolated by zero order hold + * @param input_path input path to resample + * @param resampled_arclength arclength that contains length of each resampling points from initial + * point + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample + * longitudinal and lateral velocity. Otherwise, it uses linear interpolation + * @return resampled path + */ +autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, + const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); + +/** + * @brief A resampling function for a path with lane id. Note that in a default setting, position xy + * are resampled by spline interpolation, position z are resampled by linear interpolation, + * longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is + * resampled by linear interpolation. Orientation of the resampled path are calculated by a + * forward difference method based on the interpolated position x and y. Moreover, lane_ids + * and is_final are also interpolated by zero order hold + * @param input_path input path to resample + * @param resampled_interval resampling interval point + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample + * longitudinal and lateral velocity. Otherwise, it uses linear interpolation + * @param resample_input_path_stop_point If true, resample closest stop point in input path + * @return resampled path + */ +autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, + const double resample_interval, const bool use_akima_spline_for_xy = false, + const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true, + const bool resample_input_path_stop_point = true); + +/** + * @brief A resampling function for a path. Note that in a default setting, position xy are + * resampled by spline interpolation, position z are resampled by linear interpolation, + * longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is + * resampled by linear interpolation. Orientation of the resampled path are calculated by a + * forward difference method based on the interpolated position x and y. + * @param input_path input path to resample + * @param resampled_arclength arclength that contains length of each resampling points from initial + * point + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample + * longitudinal and lateral velocity. Otherwise, it uses linear interpolation + * @return resampled path + */ +autoware_planning_msgs::msg::Path resamplePath( + const autoware_planning_msgs::msg::Path & input_path, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, + const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); + +/** + * @brief A resampling function for a path. Note that in a default setting, position xy + * are resampled by spline interpolation, position z are resampled by linear interpolation, + * longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is + * resampled by linear interpolation. Orientation of the resampled path are calculated by a + * forward difference method based on the interpolated position x and y. + * @param input_path input path to resample + * @param resampled_interval resampling interval point + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample + * longitudinal and lateral velocity. Otherwise, it uses linear interpolation + * @param resample_input_path_stop_point If true, resample closest stop point in input path + * @return resampled path + */ +autoware_planning_msgs::msg::Path resamplePath( + const autoware_planning_msgs::msg::Path & input_path, const double resample_interval, + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, + const bool use_zero_order_hold_for_twist = true, + const bool resample_input_path_stop_point = true); + +/** + * @brief A resampling function for a trajectory. Note that in a default setting, position xy are + * resampled by spline interpolation, position z are resampled by linear interpolation, twist + * informaiton(velocity and acceleration) are resampled by zero_order_hold, and heading rate + * is resampled by linear interpolation. The rest of the category is resampled by linear + * interpolation. Orientation of the resampled path are calculated by a forward difference + * method based on the interpolated position x and y. + * @param input_trajectory input trajectory to resample + * @param resampled_arclength arclength that contains length of each resampling points from initial + * point + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @param use_zero_order_hold_for_twist If true, it uses zero_order_hold to resample + * longitudinal, lateral velocity and acceleration. Otherwise, it uses linear interpolation + * @return resampled trajectory + */ +autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, + const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true); + +/** + * @brief A resampling function for a trajectory. This function resamples closest stop point, + * terminal point and points by resample interval. Note that in a default setting, position + * xy are resampled by spline interpolation, position z are resampled by linear interpolation, twist + * informaiton(velocity and acceleration) are resampled by zero_order_hold, and heading rate + * is resampled by linear interpolation. The rest of the category is resampled by linear + * interpolation. Orientation of the resampled path are calculated by a forward difference + * method based on the interpolated position x and y. + * @param input_trajectory input trajectory to resample + * @param resampled_interval resampling interval + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and + * y. Otherwise, it uses spline interpolation + * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. + * Otherwise, it uses spline interpolation + * @param use_zero_order_hold_for_twist If true, it uses zero_order_hold to resample + * longitudinal, lateral velocity and acceleration. Otherwise, it uses linear interpolation + * @param resample_input_trajectory_stop_point If true, resample closest stop point in input + * trajectory + * @return resampled trajectory + */ +autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double resample_interval, + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, + const bool use_zero_order_hold_for_twist = true, + const bool resample_input_trajectory_stop_point = true); +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp new file mode 100644 index 00000000..e15375d7 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/resample/resample_utils.hpp @@ -0,0 +1,127 @@ +// 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__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ +#define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ + +#include "autoware_utils/system/backtrace.hpp" + +#include +#include +#include + +#include + +namespace resample_utils +{ +constexpr double close_s_threshold = 1e-6; + +static inline rclcpp::Logger get_logger() +{ + constexpr const char * logger{"autoware_motion_utils.resample_utils"}; + return rclcpp::get_logger(logger); +} + +template +bool validate_size(const T & points) +{ + return points.size() >= 2; +} + +template +bool validate_resampling_range(const T & points, const std::vector & resampling_intervals) +{ + const double points_length = autoware::motion_utils::calcArcLength(points); + return points_length >= resampling_intervals.back(); +} + +template +bool validate_points_duplication(const T & points) +{ + for (size_t i = 0; i < points.size() - 1; ++i) { + const auto & curr_pt = autoware_utils::get_point(points.at(i)); + const auto & next_pt = autoware_utils::get_point(points.at(i + 1)); + const double ds = autoware_utils::calc_distance2d(curr_pt, next_pt); + if (ds < close_s_threshold) { + return false; + } + } + + return true; +} + +template +bool validate_arguments(const T & input_points, const std::vector & resampling_intervals) +{ + // Check size of the arguments + if (!validate_size(input_points)) { + RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2"); + autoware_utils::print_backtrace(); + return false; + } + if (!validate_size(resampling_intervals)) { + RCLCPP_DEBUG( + get_logger(), "invalid argument: The number of resampling intervals is less than 2"); + autoware_utils::print_backtrace(); + return false; + } + + // Check resampling range + if (!validate_resampling_range(input_points, resampling_intervals)) { + RCLCPP_DEBUG(get_logger(), "invalid argument: resampling interval is longer than input points"); + autoware_utils::print_backtrace(); + return false; + } + + // Check duplication + if (!validate_points_duplication(input_points)) { + RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points"); + autoware_utils::print_backtrace(); + return false; + } + + return true; +} + +template +bool validate_arguments(const T & input_points, const double resampling_interval) +{ + // Check size of the arguments + if (!validate_size(input_points)) { + RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2"); + autoware_utils::print_backtrace(); + return false; + } + + // check resampling interval + if (resampling_interval < autoware::motion_utils::overlap_threshold) { + RCLCPP_DEBUG( + get_logger(), "invalid argument: resampling interval is less than %f", + autoware::motion_utils::overlap_threshold); + autoware_utils::print_backtrace(); + return false; + } + + // Check duplication + if (!validate_points_duplication(input_points)) { + RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points"); + autoware_utils::print_backtrace(); + return false; + } + + return true; +} +} // namespace resample_utils + +#endif // AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp new file mode 100644 index 00000000..a00b1d27 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/conversion.hpp @@ -0,0 +1,121 @@ +// 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__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ + +#include "autoware_internal_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" +#include "autoware_planning_msgs/msg/detail/path__struct.hpp" +#include "autoware_planning_msgs/msg/detail/trajectory__struct.hpp" +#include "autoware_planning_msgs/msg/detail/trajectory_point__struct.hpp" +#include "std_msgs/msg/header.hpp" + +#include + +namespace autoware::motion_utils +{ +using TrajectoryPoints = std::vector; + +/** + * @brief Convert std::vector to + * autoware_planning_msgs::msg::Trajectory. This function is temporarily added for porting to + * autoware_msgs. We should consider whether to remove this function after the porting is done. + * @attention This function just clips + * std::vector up to the capacity of Trajectory. + * Therefore, the error handling out of this function is necessary if the size of the input greater + * than the capacity. + * @todo Decide how to handle the situation that we need to use the trajectory with the size of + * points larger than the capacity. (Tier IV) + */ +autoware_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory, + const std_msgs::msg::Header & header = std_msgs::msg::Header{}); + +/** + * @brief Convert autoware_planning_msgs::msg::Trajectory to + * std::vector. + */ +std::vector convertToTrajectoryPointArray( + const autoware_planning_msgs::msg::Trajectory & trajectory); + +template +autoware_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToPath can be used."); + throw std::logic_error("Only specializations of convertToPath can be used."); +} + +template <> +inline autoware_planning_msgs::msg::Path convertToPath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input) +{ + autoware_planning_msgs::msg::Path output{}; + output.header = input.header; + output.left_bound = input.left_bound; + output.right_bound = input.right_bound; + output.points.resize(input.points.size()); + for (size_t i = 0; i < input.points.size(); ++i) { + output.points.at(i) = input.points.at(i).point; + } + return output; +} + +template +TrajectoryPoints convertToTrajectoryPoints([[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToTrajectoryPoints can be used."); + throw std::logic_error("Only specializations of convertToTrajectoryPoints can be used."); +} + +template <> +inline TrajectoryPoints convertToTrajectoryPoints( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input) +{ + TrajectoryPoints output{}; + for (const auto & p : input.points) { + autoware_planning_msgs::msg::TrajectoryPoint tp; + tp.pose = p.point.pose; + tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; + // since path point doesn't have acc for now + tp.acceleration_mps2 = 0; + output.emplace_back(tp); + } + return output; +} + +template +autoware_internal_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( + [[maybe_unused]] const T & input) +{ + static_assert(sizeof(T) == 0, "Only specializations of convertToPathWithLaneId can be used."); + throw std::logic_error("Only specializations of convertToPathWithLaneId can be used."); +} + +template <> +inline autoware_internal_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( + const TrajectoryPoints & input) +{ + autoware_internal_planning_msgs::msg::PathWithLaneId output{}; + for (const auto & p : input) { + autoware_internal_planning_msgs::msg::PathPointWithLaneId pp; + pp.point.pose = p.pose; + pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; + output.points.emplace_back(pp); + } + return output; +} + +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp new file mode 100644 index 00000000..920235d5 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/interpolation.hpp @@ -0,0 +1,96 @@ +// 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__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ + +#include "autoware_utils/geometry/geometry.hpp" + +#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" + +#include + +#include +#include + +namespace autoware::motion_utils +{ +/** + * @brief An interpolation function that finds the closest interpolated point on the trajectory from + * the given pose + * @param trajectory input trajectory + * @param target_pose target_pose + * @param use_zero_order_for_twist flag to decide wether to use zero order hold interpolation for + * twist information + * @return resampled path(poses) + */ +autoware_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint( + const autoware_planning_msgs::msg::Trajectory & trajectory, + const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); + +/** + * @brief An interpolation function that finds the closest interpolated point on the path from + * the given pose + * @param path input path + * @param target_pose target_pose + * @param use_zero_order_for_twist flag to decide wether to use zero order hold interpolation for + * twist information + * @return resampled path(poses) + */ +autoware_internal_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); + +/** + * @brief An interpolation function that finds the closest interpolated point on the path that is a + * certain length away from the given pose + * @param points input path + * @param target_length length from the front point of the path + * @return resampled pose + */ +template +geometry_msgs::msg::Pose calcInterpolatedPose(const T & points, const double target_length) +{ + if (points.empty()) { + geometry_msgs::msg::Pose interpolated_pose; + return interpolated_pose; + } + + if (points.size() < 2 || target_length < 0.0) { + return autoware_utils::get_pose(points.front()); + } + + double accumulated_length = 0; + for (size_t i = 0; i < points.size() - 1; ++i) { + const auto & curr_pose = autoware_utils::get_pose(points.at(i)); + const auto & next_pose = autoware_utils::get_pose(points.at(i + 1)); + const double length = autoware_utils::calc_distance3d(curr_pose, next_pose); + if (accumulated_length + length > target_length) { + const double ratio = (target_length - accumulated_length) / std::max(length, 1e-6); + return autoware_utils::calc_interpolated_pose(curr_pose, next_pose, ratio); + } + accumulated_length += length; + } + + return autoware_utils::get_pose(points.back()); +} + +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__INTERPOLATION_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_shift.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_shift.hpp new file mode 100644 index 00000000..f4602fff --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_shift.hpp @@ -0,0 +1,71 @@ +// Copyright 2024 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__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_ + +namespace autoware::motion_utils +{ +/** + * @brief Calculates the velocity required for shifting + * @param lateral lateral distance + * @param jerk lateral jerk + * @param longitudinal_distance longitudinal distance + * @return velocity + */ +double calc_feasible_velocity_from_jerk( + const double lateral, const double jerk, const double longitudinal_distance); + +/** + * @brief Calculates the lateral distance required for shifting + * @param longitudinal longitudinal distance + * @param jerk lateral jerk + * @param velocity velocity + * @return lateral distance + */ +double calc_lateral_dist_from_jerk( + const double longitudinal, const double jerk, const double velocity); + +/** + * @brief Calculates the lateral distance required for shifting + * @param lateral lateral distance + * @param jerk lateral jerk + * @param velocity velocity + * @return longitudinal distance + */ +double calc_longitudinal_dist_from_jerk( + const double lateral, const double jerk, const double velocity); + +/** + * @brief Calculates the total time required for shifting + * @param lateral lateral distance + * @param jerk lateral jerk + * @param acc lateral acceleration + * @return time + */ +double calc_shift_time_from_jerk(const double lateral, const double jerk, const double acc); + +/** + * @brief Calculates the required jerk from lateral/longitudinal distance + * @param lateral lateral distance + * @param longitudinal longitudinal distance + * @param velocity velocity + * @return jerk + */ +double calc_jerk_from_lat_lon_distance( + const double lateral, const double longitudinal, const double velocity); + +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_SHIFT_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp new file mode 100644 index 00000000..d23c0851 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/path_with_lane_id.hpp @@ -0,0 +1,46 @@ +// 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__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ + +#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp" +#include + +#include +#include +namespace autoware::motion_utils +{ +std::optional> getPathIndexRangeWithLaneId( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); + +size_t findNearestIndexFromLaneId( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & pos, const int64_t lane_id); + +size_t findNearestSegmentIndexFromLaneId( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & pos, const int64_t lane_id); + +// @brief Calculates the path to be followed by the rear wheel center in order to make the vehicle +// center follow the input path +// @param [in] path with position to be followed by the center of the vehicle +// @param [out] PathWithLaneId to be followed by the rear wheel center follow to make the vehicle +// center follow the input path NOTE: rear_to_cog is supposed to be positive +autoware_internal_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, + const bool enable_last_point_compensation = true); +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp new file mode 100644 index 00000000..3e80499c --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp @@ -0,0 +1,2526 @@ +// 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__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ + +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/geometry/pose_deviation.hpp" +#include "autoware_utils/math/constants.hpp" +#include "autoware_utils/system/backtrace.hpp" + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_utils +{ +static inline rclcpp::Logger get_logger() +{ + constexpr const char * logger{"autoware_motion_utils.trajectory"}; + return rclcpp::get_logger(logger); +} + +/** + * @brief validate if points container is empty or not + * @param points points of trajectory, path, ... + */ +template +void validateNonEmpty(const T & points) +{ + if (points.empty()) { + autoware_utils::print_backtrace(); + throw std::invalid_argument("[autoware_motion_utils] validateNonEmpty(): Points is empty."); + } +} + +extern template void validateNonEmpty>( + const std::vector &); +extern template void +validateNonEmpty>( + const std::vector &); +extern template void validateNonEmpty>( + const std::vector &); + +/** + * @brief validate a point is in a non-sharp angle between two points or not + * @param point1 front point + * @param point2 point to be validated + * @param point3 back point + */ +template +void validateNonSharpAngle( + const T & point1, const T & point2, const T & point3, + const double angle_threshold = autoware_utils::pi / 4) +{ + const auto p1 = autoware_utils::get_point(point1); + const auto p2 = autoware_utils::get_point(point2); + const auto p3 = autoware_utils::get_point(point3); + + const std::vector vec_1to2 = {p2.x - p1.x, p2.y - p1.y, p2.z - p1.z}; + const std::vector vec_3to2 = {p2.x - p3.x, p2.y - p3.y, p2.z - p3.z}; + const auto product = std::inner_product(vec_1to2.begin(), vec_1to2.end(), vec_3to2.begin(), 0.0); + + const auto dist_1to2 = autoware_utils::calc_distance3d(p1, p2); + const auto dist_3to2 = autoware_utils::calc_distance3d(p3, p2); + + constexpr double epsilon = 1e-3; + if (std::cos(angle_threshold) < product / dist_1to2 / dist_3to2 + epsilon) { + autoware_utils::print_backtrace(); + throw std::invalid_argument( + "[autoware_motion_utils] validateNonSharpAngle(): Too sharp angle."); + } +} + +/** + * @brief checks whether a path of trajectory has forward driving direction + * @param points points of trajectory, path, ... + * @return (forward / backward) driving (true / false) + */ +template +std::optional isDrivingForward(const T & points) +{ + if (points.size() < 2) { + return std::nullopt; + } + + // check the first point direction + const auto & first_pose = autoware_utils::get_pose(points.at(0)); + const auto & second_pose = autoware_utils::get_pose(points.at(1)); + + return autoware_utils::is_driving_forward(first_pose, second_pose); +} + +extern template std::optional +isDrivingForward>( + const std::vector &); +extern template std::optional +isDrivingForward>( + const std::vector &); +extern template std::optional +isDrivingForward>( + const std::vector &); + +/** + * @brief checks whether a path of trajectory has forward driving direction using its longitudinal + * velocity + * @param points_with_twist points of trajectory, path, ... (with velocity) + * @return (forward / backward) driving (true, false, none "if velocity is zero") + */ +template +std::optional isDrivingForwardWithTwist(const T & points_with_twist) +{ + if (points_with_twist.empty()) { + return std::nullopt; + } + if (points_with_twist.size() == 1) { + if (0.0 < autoware_utils::get_longitudinal_velocity(points_with_twist.front())) { + return true; + } + if (0.0 > autoware_utils::get_longitudinal_velocity(points_with_twist.front())) { + return false; + } + return std::nullopt; + } + + return isDrivingForward(points_with_twist); +} + +extern template std::optional +isDrivingForwardWithTwist>( + const std::vector &); +extern template std::optional +isDrivingForwardWithTwist>( + const std::vector &); +extern template std::optional +isDrivingForwardWithTwist>( + const std::vector &); + +/** + * @brief remove overlapping points through points container. + * Overlapping is determined by calculating the distance between 2 consecutive points. + * If the distance between them is less than a threshold, they will be considered overlapping. + * @param points points of trajectory, path, ... + * @param start_idx index to start the overlap remove calculation from through the points + * container. Indices before that index will be considered non-overlapping. Default = 0 + * @return points container without overlapping points + */ +template +T removeOverlapPoints(const T & points, const size_t start_idx = 0) +{ + if (points.size() < start_idx + 1) { + return points; + } + + T dst; + dst.reserve(points.size()); + + for (size_t i = 0; i <= start_idx; ++i) { + dst.push_back(points.at(i)); + } + + constexpr double eps = 1.0E-08; + for (size_t i = start_idx + 1; i < points.size(); ++i) { + const auto prev_p = autoware_utils::get_point(dst.back()); + const auto curr_p = autoware_utils::get_point(points.at(i)); + if (std::abs(prev_p.x - curr_p.x) < eps && std::abs(prev_p.y - curr_p.y) < eps) { + continue; + } + dst.push_back(points.at(i)); + } + + return dst; +} + +extern template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx = 0); +extern template std::vector +removeOverlapPoints>( + const std::vector & points, + const size_t start_idx = 0); +extern template std::vector +removeOverlapPoints>( + const std::vector & points, + const size_t start_idx = 0); + +/** + * @brief search through points container from specified start and end indices about first matching + * index of a zero longitudinal velocity point. + * @param points_with_twist points of trajectory, path, ... (with velocity) + * @param src_idx start index of the search + * @param dst_idx end index of the search + * @return first matching index of a zero velocity point inside the points container. + */ +template +std::optional searchZeroVelocityIndex( + const T & points_with_twist, const size_t src_idx, const size_t dst_idx) +{ + try { + validateNonEmpty(points_with_twist); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return {}; + } + + constexpr double epsilon = 1e-3; + for (size_t i = src_idx; i < dst_idx; ++i) { + if (std::fabs(points_with_twist.at(i).longitudinal_velocity_mps) < epsilon) { + return i; + } + } + + return {}; +} + +extern template std::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist, + const size_t src_idx, const size_t dst_idx); + +/** + * @brief search through points container from specified start index till end of points container + * about first matching index of a zero longitudinal velocity point. + * @param points_with_twist points of trajectory, path, ... (with velocity) + * @param src_idx start index of the search + * @return first matching index of a zero velocity point inside the points container. + */ +template +std::optional searchZeroVelocityIndex(const T & points_with_twist, const size_t src_idx) +{ + try { + validateNonEmpty(points_with_twist); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return {}; + } + + return searchZeroVelocityIndex(points_with_twist, src_idx, points_with_twist.size()); +} + +extern template std::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist, + const size_t src_idx); + +/** + * @brief search through points container from its start to end about first matching index of a zero + * longitudinal velocity point. + * @param points_with_twist points of trajectory, path, ... (with velocity) + * @return first matching index of a zero velocity point inside the points container. + */ +template +std::optional searchZeroVelocityIndex(const T & points_with_twist) +{ + return searchZeroVelocityIndex(points_with_twist, 0, points_with_twist.size()); +} + +extern template std::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist); + +/** + * @brief find nearest point index through points container for a given point. + * Finding nearest point is determined by looping through the points container, + * and calculating the 2D squared distance between each point in the container and the given point. + * The index of the point with minimum distance and yaw deviation comparing to the given point will + * be returned. + * @param points points of trajectory, path, ... + * @param point given point + * @return index of nearest point + */ +template +size_t findNearestIndex(const T & points, const geometry_msgs::msg::Point & point) +{ + validateNonEmpty(points); + + double min_dist = std::numeric_limits::max(); + size_t min_idx = 0; + + for (size_t i = 0; i < points.size(); ++i) { + const auto dist = autoware_utils::calc_squared_distance2d(points.at(i), point); + if (dist < min_dist) { + min_dist = dist; + min_idx = i; + } + } + return min_idx; +} + +extern template size_t findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +extern template size_t +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +extern template size_t findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); + +/** + * @brief find nearest point index through points container for a given pose. + * Finding nearest point is determined by looping through the points container, + * and finding the nearest point to the given pose in terms of squared 2D distance and yaw + * deviation. The index of the point with minimum distance and yaw deviation comparing to the given + * pose will be returned. + * @param points points of trajectory, path, ... + * @param pose given pose + * @param max_dist max distance used to get squared distance for finding the nearest point to given + * pose + * @param max_yaw max yaw used for finding nearest point to given pose + * @return index of nearest point (index or none if not found) + */ +template +std::optional findNearestIndex( + const T & points, const geometry_msgs::msg::Pose & pose, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return {}; + } + + const double max_squared_dist = max_dist * max_dist; + + double min_squared_dist = std::numeric_limits::max(); + bool is_nearest_found = false; + size_t min_idx = 0; + + for (size_t i = 0; i < points.size(); ++i) { + const auto squared_dist = autoware_utils::calc_squared_distance2d(points.at(i), pose); + if (squared_dist > max_squared_dist || squared_dist >= min_squared_dist) { + continue; + } + + const auto yaw = + autoware_utils::calc_yaw_deviation(autoware_utils::get_pose(points.at(i)), pose); + if (std::fabs(yaw) > max_yaw) { + continue; + } + + min_squared_dist = squared_dist; + min_idx = i; + is_nearest_found = true; + } + + if (is_nearest_found) { + return min_idx; + } + return std::nullopt; +} + +extern template std::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template std::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template std::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); + +/** + * @brief calculate longitudinal offset (length along trajectory from seg_idx point to nearest point + * to p_target on trajectory). If seg_idx point is after that nearest point, length is negative. + * Segment is straight path between two continuous points of trajectory. + * @param points points of trajectory, path, ... + * @param seg_idx segment index of point at beginning of length + * @param p_target target point at end of length + * @param throw_exception flag to enable/disable exception throwing + * @return signed length + */ +template +double calcLongitudinalOffsetToSegment( + const T & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + const bool throw_exception = false) +{ + if (seg_idx >= points.size() - 1) { + const std::string error_message( + "[autoware_motion_utils] " + std::string(__func__) + + ": Failed to calculate longitudinal offset because the given segment index is out of the " + "points size."); + autoware_utils::print_backtrace(); + if (throw_exception) { + throw std::out_of_range(error_message); + } + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); + return std::nan(""); + } + + const auto overlap_removed_points = removeOverlapPoints(points, seg_idx); + + if (throw_exception) { + validateNonEmpty(overlap_removed_points); + } else { + try { + validateNonEmpty(overlap_removed_points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return std::nan(""); + } + } + + if (seg_idx >= overlap_removed_points.size() - 1) { + const std::string error_message( + "[autoware_motion_utils] " + std::string(__func__) + + ": Longitudinal offset calculation is not supported for the same points."); + autoware_utils::print_backtrace(); + if (throw_exception) { + throw std::runtime_error(error_message); + } + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); + return std::nan(""); + } + + const auto p_front = autoware_utils::get_point(overlap_removed_points.at(seg_idx)); + const auto p_back = autoware_utils::get_point(overlap_removed_points.at(seg_idx + 1)); + + const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0}; + const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0}; + + return segment_vec.dot(target_vec) / segment_vec.norm(); +} + +extern template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); +extern template double calcLongitudinalOffsetToSegment< + std::vector>( + const std::vector & points, + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + const bool throw_exception = false); +extern template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); + +/** + * @brief find nearest segment index to point. + * Segment is straight path between two continuous points of trajectory. + * When point is on a trajectory point whose index is nearest_idx, return nearest_idx - 1 + * @param points points of trajectory, path, ... + * @param point point to which to find nearest segment index + * @return nearest index + */ +template +size_t findNearestSegmentIndex(const T & points, const geometry_msgs::msg::Point & point) +{ + const size_t nearest_idx = findNearestIndex(points, point); + + if (nearest_idx == 0) { + return 0; + } + if (nearest_idx == points.size() - 1) { + return points.size() - 2; + } + + const double signed_length = calcLongitudinalOffsetToSegment(points, nearest_idx, point); + + if (signed_length <= 0) { + return nearest_idx - 1; + } + + return nearest_idx; +} + +extern template size_t findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +extern template size_t +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +extern template size_t +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); + +/** + * @brief find nearest segment index to pose + * Segment is straight path between two continuous points of trajectory. + * When pose is on a trajectory point whose index is nearest_idx, return nearest_idx - 1 + * @param points points of trajectory, path, .. + * @param pose pose to which to find nearest segment index + * @param max_dist max distance used for finding the nearest index to given pose + * @param max_yaw max yaw used for finding nearest index to given pose + * @return nearest index + */ +template +std::optional findNearestSegmentIndex( + const T & points, const geometry_msgs::msg::Pose & pose, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()) +{ + const auto nearest_idx = findNearestIndex(points, pose, max_dist, max_yaw); + + if (!nearest_idx) { + return std::nullopt; + } + + if (*nearest_idx == 0) { + return 0; + } + if (*nearest_idx == points.size() - 1) { + return points.size() - 2; + } + + const double signed_length = calcLongitudinalOffsetToSegment(points, *nearest_idx, pose.position); + + if (signed_length <= 0) { + return *nearest_idx - 1; + } + + return *nearest_idx; +} + +extern template std::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template std::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template std::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); + +/** + * @brief calculate lateral offset from p_target (length from p_target to trajectory) using given + * segment index. Segment is straight path between two continuous points of trajectory. + * @param points points of trajectory, path, ... + * @param p_target target point + * @param seg_idx segment index of point at beginning of length + * @param throw_exception flag to enable/disable exception throwing + * @return length (unsigned) + */ +template +double calcLateralOffset( + const T & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, + const bool throw_exception = false) +{ + const auto overlap_removed_points = removeOverlapPoints(points, 0); + + if (throw_exception) { + validateNonEmpty(overlap_removed_points); + } else { + try { + validateNonEmpty(overlap_removed_points); + } catch (const std::exception & e) { + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + e.what()); + return std::nan(""); + } + } + + if (overlap_removed_points.size() == 1) { + const std::string error_message( + "[autoware_motion_utils] " + std::string(__func__) + + ": Lateral offset calculation is not supported for the same points."); + autoware_utils::print_backtrace(); + if (throw_exception) { + throw std::runtime_error(error_message); + } + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); + return std::nan(""); + } + + const auto p_indices = overlap_removed_points.size() - 2; + const auto p_front_idx = (p_indices > seg_idx) ? seg_idx : p_indices; + const auto p_back_idx = p_front_idx + 1; + + const auto p_front = autoware_utils::get_point(overlap_removed_points.at(p_front_idx)); + const auto p_back = autoware_utils::get_point(overlap_removed_points.at(p_back_idx)); + + const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; + const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0}; + + const Eigen::Vector3d cross_vec = segment_vec.cross(target_vec); + return cross_vec(2) / segment_vec.norm(); +} + +extern template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, + const bool throw_exception = false); +extern template double +calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, + const bool throw_exception = false); +extern template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, + const bool throw_exception = false); + +/** + * @brief calculate lateral offset from p_target (length from p_target to trajectory). + * The function gets the nearest segment index between the points of trajectory and the given target + * point, then uses that segment index to calculate lateral offset. Segment is straight path between + * two continuous points of trajectory. + * @param points points of trajectory, path, ... + * @param p_target target point + * @param throw_exception flag to enable/disable exception throwing + * @return length (unsigned) + */ +template +double calcLateralOffset( + const T & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false) +{ + const auto overlap_removed_points = removeOverlapPoints(points, 0); + + if (throw_exception) { + validateNonEmpty(overlap_removed_points); + } else { + try { + validateNonEmpty(overlap_removed_points); + } catch (const std::exception & e) { + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + e.what()); + return std::nan(""); + } + } + + if (overlap_removed_points.size() == 1) { + const std::string error_message( + "[autoware_motion_utils] " + std::string(__func__) + + ": Lateral offset calculation is not supported for the same points."); + autoware_utils::print_backtrace(); + if (throw_exception) { + throw std::runtime_error(error_message); + } + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); + return std::nan(""); + } + + const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, p_target); + return calcLateralOffset(points, p_target, seg_idx, throw_exception); +} + +extern template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); +extern template double +calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); +extern template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); + +/** + * @brief calculate length of 2D distance between two points, specified by start and end points + * indicies through points container. + * @param points points of trajectory, path, ... + * @param src_idx index of start point + * @param dst_idx index of end point + * @return length of distance between two points. + * Length is positive if dst_idx is greater that src_idx (i.e. after it in trajectory, path, ...) + * and negative otherwise. + */ +template +double calcSignedArcLength(const T & points, const size_t src_idx, const size_t dst_idx) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return 0.0; + } + + if (src_idx > dst_idx) { + return -calcSignedArcLength(points, dst_idx, src_idx); + } + + double dist_sum = 0.0; + for (size_t i = src_idx; i < dst_idx; ++i) { + dist_sum += autoware_utils::calc_distance2d(points.at(i), points.at(i + 1)); + } + return dist_sum; +} + +extern template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); + +/** + * @brief Computes the partial sums of the elements in the sub-ranges of the range [src_idx, + * dst_idx) and return these sum as vector + * @param points points of trajectory, path, ... + * @param src_idx index of start point + * @param dst_idx index of end point + * @return partial sums container + */ +template +std::vector calcSignedArcLengthPartialSum( + const T & points, const size_t src_idx, const size_t dst_idx) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return {}; + } + + if (src_idx + 1 > dst_idx) { + auto copied = points; + std::reverse(copied.begin(), copied.end()); + return calcSignedArcLengthPartialSum(points, dst_idx, src_idx); + } + + std::vector partial_dist; + partial_dist.reserve(dst_idx - src_idx); + + double dist_sum = 0.0; + partial_dist.push_back(dist_sum); + for (size_t i = src_idx; i < dst_idx - 1; ++i) { + dist_sum += autoware_utils::calc_distance2d(points.at(i), points.at(i + 1)); + partial_dist.push_back(dist_sum); + } + return partial_dist; +} + +extern template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +extern template std::vector calcSignedArcLengthPartialSum< + std::vector>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); +extern template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); + +/** + * @brief calculate length of 2D distance between two points, specified by start point and end point + * index of points container. + * @param points points of trajectory, path, ... + * @param src_point start point + * @param dst_idx index of end point + * @return length of distance between two points. + * Length is positive if destination point associated to dst_idx is greater that src_idx (i.e. after + * it in trajectory, path, ...) and negative otherwise. + */ +template +double calcSignedArcLength( + const T & points, const geometry_msgs::msg::Point & src_point, const size_t dst_idx) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return 0.0; + } + + const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); + + const double signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_idx); + const double signed_length_src_offset = + calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); + + return signed_length_on_traj - signed_length_src_offset; +} + +extern template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector &, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector &, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); + +/** + * @brief calculate length of 2D distance between two points, specified by start index of points + * container and end point. + * @param points points of trajectory, path, ... + * @param src_idx index of start point + * @param dst_point end point + * @return length of distance between two points + * Length is positive if destination point is greater that source point associated to src_idx (i.e. + * after it in trajectory, path, ...) and negative otherwise. + */ +template +double calcSignedArcLength( + const T & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return 0.0; + } + + return -calcSignedArcLength(points, dst_point, src_idx); +} + +extern template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); +extern template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point); +extern template double +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); + +/** + * @brief calculate length of 2D distance between two points, specified by start point and end + * point. + * @param points points of trajectory, path, ... + * @param src_point start point + * @param dst_point end point + * @return length of distance between two points. + * Length is positive if destination point is greater that source point (i.e. after it in + * trajectory, path, ...) and negative otherwise. + * + */ +template +double calcSignedArcLength( + const T & points, const geometry_msgs::msg::Point & src_point, + const geometry_msgs::msg::Point & dst_point) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return 0.0; + } + + const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); + const size_t dst_seg_idx = findNearestSegmentIndex(points, dst_point); + + const double signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_seg_idx); + const double signed_length_src_offset = + calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); + const double signed_length_dst_offset = + calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); + + return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; +} + +extern template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); + +/** + * @brief calculate length of 2D distance for whole points container, from its start to its end. + * @param points points of trajectory, path, ... + * @return length of 2D distance for points container + */ +template +double calcArcLength(const T & points) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return 0.0; + } + + return calcSignedArcLength(points, 0, points.size() - 1); +} + +extern template double calcArcLength>( + const std::vector & points); +extern template double +calcArcLength>( + const std::vector & points); +extern template double calcArcLength>( + const std::vector & points); + +/** + * @brief calculate curvature through points container. + * The method used for calculating the curvature is using 3 consecutive points through the points + * container. Then the curvature is the reciprocal of the radius of the circle that passes through + * these three points. + * @details more details here : https://en.wikipedia.org/wiki/Menger_curvature + * @param points points of trajectory, path, ... + * @return calculated curvature container through points container + */ +template +std::vector calc_curvature(const T & points) +{ + std::vector curvature_vec(points.size(), 0.0); + if (points.size() < 3) { + return curvature_vec; + } + + for (size_t i = 1; i < points.size() - 1; ++i) { + const auto p1 = autoware_utils::get_point(points.at(i - 1)); + const auto p2 = autoware_utils::get_point(points.at(i)); + const auto p3 = autoware_utils::get_point(points.at(i + 1)); + curvature_vec.at(i) = (autoware_utils::calc_curvature(p1, p2, p3)); + } + curvature_vec.at(0) = curvature_vec.at(1); + curvature_vec.at(curvature_vec.size() - 1) = curvature_vec.at(curvature_vec.size() - 2); + + return curvature_vec; +} + +extern template std::vector +calc_curvature>( + const std::vector & points); +extern template std::vector +calc_curvature>( + const std::vector & points); +extern template std::vector +calc_curvature>( + const std::vector & points); + +/** + * @brief calculate curvature through points container and length of 2d distance for segment used + * for curvature calculation. The method used for calculating the curvature is using 3 consecutive + * points through the points container. Then the curvature is the reciprocal of the radius of the + * circle that passes through these three points. Then length of 2D distance of these points is + * calculated + * @param points points of trajectory, path, ... + * @return Container of pairs, calculated curvature and length of 2D distance for segment used for + * curvature calculation + */ +template +std::vector>> calc_curvatureAndSegmentLength( + const T & points) +{ + // segment length is pair of segment length between {p1, p2} and {p2, p3} + std::vector>> curvature_and_segment_length_vec; + curvature_and_segment_length_vec.reserve(points.size()); + curvature_and_segment_length_vec.emplace_back(0.0, std::make_pair(0.0, 0.0)); + for (size_t i = 1; i < points.size() - 1; ++i) { + const auto p1 = autoware_utils::get_point(points.at(i - 1)); + const auto p2 = autoware_utils::get_point(points.at(i)); + const auto p3 = autoware_utils::get_point(points.at(i + 1)); + const double curvature = autoware_utils::calc_curvature(p1, p2, p3); + + // The first point has only the next point, so put the distance to that point. + // In other words, assign the first segment length at the second point to the + // second_segment_length at the first point. + if (i == 1) { + curvature_and_segment_length_vec.at(0).second.second = + autoware_utils::calc_distance2d(p1, p2); + } + + // The second_segment_length of the previous point and the first segment length of the current + // point are equal. + const std::pair arc_length{ + curvature_and_segment_length_vec.back().second.second, + autoware_utils::calc_distance2d(p2, p3)}; + + curvature_and_segment_length_vec.emplace_back(curvature, arc_length); + } + + // set to the last point + curvature_and_segment_length_vec.emplace_back( + 0.0, std::make_pair(curvature_and_segment_length_vec.back().second.second, 0.0)); + + return curvature_and_segment_length_vec; +} + +extern template std::vector>> +calc_curvatureAndSegmentLength>( + const std::vector & points); +extern template std::vector>> +calc_curvatureAndSegmentLength< + std::vector>( + const std::vector & points); +extern template std::vector>> +calc_curvatureAndSegmentLength>( + const std::vector & points); + +/** + * @brief calculate length of 2D distance between given start point index in points container and + * first point in container with zero longitudinal velocity + * @param points_with_twist points of trajectory, path, ... (with velocity) + * @return Length of 2D distance between start point index in points container and first point in + * container with zero longitudinal velocity + */ +template +std::optional calcDistanceToForwardStopPoint( + const T & points_with_twist, const size_t src_idx = 0) +{ + try { + validateNonEmpty(points_with_twist); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return {}; + } + + const auto closest_stop_idx = + searchZeroVelocityIndex(points_with_twist, src_idx, points_with_twist.size()); + if (!closest_stop_idx) { + return std::nullopt; + } + + return std::max(0.0, calcSignedArcLength(points_with_twist, src_idx, *closest_stop_idx)); +} + +extern template std::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const size_t src_idx = 0); + +/** + * @brief calculate the point offset from source point index along the trajectory (or path) (points + * container) + * @param points points of trajectory, path, ... + * @param src_idx index of source point + * @param offset length of offset from source point + * @param throw_exception flag to enable/disable exception throwing + * @return offset point + */ +template +std::optional calcLongitudinalOffsetPoint( + const T & points, const size_t src_idx, const double offset, const bool throw_exception = false) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return {}; + } + + if (points.size() - 1 < src_idx) { + const std::string error_message( + "[autoware_motion_utils] " + std::string(__func__) + + " error: The given source index is out of the points size. Failed to calculate longitudinal " + "offset."); + autoware_utils::print_backtrace(); + if (throw_exception) { + throw std::out_of_range(error_message); + } + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); + return {}; + } + + if (points.size() == 1) { + return {}; + } + + if (src_idx + 1 == points.size() && offset == 0.0) { + return autoware_utils::get_point(points.at(src_idx)); + } + + if (offset < 0.0) { + auto reverse_points = points; + std::reverse(reverse_points.begin(), reverse_points.end()); + return calcLongitudinalOffsetPoint( + reverse_points, reverse_points.size() - src_idx - 1, -offset); + } + + double dist_sum = 0.0; + + for (size_t i = src_idx; i < points.size() - 1; ++i) { + const auto & p_front = points.at(i); + const auto & p_back = points.at(i + 1); + + const auto dist_segment = autoware_utils::calc_distance2d(p_front, p_back); + dist_sum += dist_segment; + + const auto dist_res = offset - dist_sum; + if (dist_res <= 0.0) { + return autoware_utils::calc_interpolated_point( + p_back, p_front, std::abs(dist_res / dist_segment)); + } + } + + // not found (out of range) + return {}; +} + +extern template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception = false); +extern template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const size_t src_idx, const double offset, const bool throw_exception = false); +extern template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception = false); + +/** + * @brief calculate the point offset from source point along the trajectory (or path) (points + * container) + * @param points points of trajectory, path, ... + * @param src_point source point + * @param offset length of offset from source point + * @return offset point + */ +template +std::optional calcLongitudinalOffsetPoint( + const T & points, const geometry_msgs::msg::Point & src_point, const double offset) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "Failed to calculate longitudinal offset: %s", e.what()); + return {}; + } + + if (offset < 0.0) { + auto reverse_points = points; + std::reverse(reverse_points.begin(), reverse_points.end()); + return calcLongitudinalOffsetPoint(reverse_points, src_point, -offset); + } + + const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); + const double signed_length_src_offset = + calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); + + return calcLongitudinalOffsetPoint(points, src_seg_idx, offset + signed_length_src_offset); +} + +extern template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); +extern template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); +extern template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); + +/** + * @brief calculate the point offset from source point index along the trajectory (or path) (points + * container) + * @param points points of trajectory, path, ... + * @param src_idx index of source point + * @param offset length of offset from source point + * @param set_orientation_from_position_direction set orientation by spherical interpolation if + * false + * @return offset pose + */ +template +std::optional calcLongitudinalOffsetPose( + const T & points, const size_t src_idx, const double offset, + const bool set_orientation_from_position_direction = true, const bool throw_exception = false) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "Failed to calculate longitudinal offset: %s", e.what()); + return {}; + } + + if (points.size() - 1 < src_idx) { + const std::string error_message( + "[autoware_motion_utils] " + std::string(__func__) + + " error: The given source index is out of the points size. Failed to calculate longitudinal " + "offset."); + autoware_utils::print_backtrace(); + if (throw_exception) { + throw std::out_of_range(error_message); + } + RCLCPP_DEBUG(get_logger(), "%s", error_message.c_str()); + return {}; + } + + if (points.size() == 1) { + RCLCPP_DEBUG(get_logger(), "Failed to calculate longitudinal offset: points size is one."); + return {}; + } + + if (src_idx + 1 == points.size() && offset == 0.0) { + return autoware_utils::get_pose(points.at(src_idx)); + } + + if (offset < 0.0) { + auto reverse_points = points; + std::reverse(reverse_points.begin(), reverse_points.end()); + + double dist_sum = 0.0; + + for (size_t i = reverse_points.size() - src_idx - 1; i < reverse_points.size() - 1; ++i) { + const auto & p_front = reverse_points.at(i); + const auto & p_back = reverse_points.at(i + 1); + + const auto dist_segment = autoware_utils::calc_distance2d(p_front, p_back); + dist_sum += dist_segment; + + const auto dist_res = -offset - dist_sum; + if (dist_res <= 0.0) { + return autoware_utils::calc_interpolated_pose( + p_back, p_front, std::abs(dist_res / dist_segment), + set_orientation_from_position_direction); + } + } + } else { + double dist_sum = 0.0; + + for (size_t i = src_idx; i < points.size() - 1; ++i) { + const auto & p_front = points.at(i); + const auto & p_back = points.at(i + 1); + + const auto dist_segment = autoware_utils::calc_distance2d(p_front, p_back); + dist_sum += dist_segment; + + const auto dist_res = offset - dist_sum; + if (dist_res <= 0.0) { + return autoware_utils::calc_interpolated_pose( + p_front, p_back, 1.0 - std::abs(dist_res / dist_segment), + set_orientation_from_position_direction); + } + } + } + + // not found (out of range) + return {}; +} + +extern template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction = true, + const bool throw_exception = false); +extern template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const size_t src_idx, const double offset, + const bool set_orientation_from_position_direction = true, const bool throw_exception = false); +extern template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction = true, + const bool throw_exception = false); + +/** + * @brief calculate the point offset from source point along the trajectory (or path) (points + * container) + * @param points points of trajectory, path, ... + * @param src_point source point + * @param offset length of offset from source point + * @param set_orientation_from_position_direction set orientation by spherical interpolation if + * false + * @return offset pose + */ +template +std::optional calcLongitudinalOffsetPose( + const T & points, const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction = true) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return {}; + } + + const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); + const double signed_length_src_offset = + calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); + + return calcLongitudinalOffsetPose( + points, src_seg_idx, offset + signed_length_src_offset, + set_orientation_from_position_direction); +} + +extern template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction = true); +extern template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction = true); +extern template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction = true); + +/** + * @brief insert a point in points container (trajectory, path, ...) using segment id + * @param seg_idx segment index of point at beginning of length + * @param p_target point to be inserted + * @param points output points of trajectory, path, ... + * @param overlap_threshold distance threshold, used to check if the inserted point is between start + * and end of nominated segment to be added in. + * @return index of segment id, where point is inserted + */ +template +std::optional insertTargetPoint( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, T & points, + const double overlap_threshold = 1e-3) +{ + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return {}; + } + + // invalid segment index + if (seg_idx + 1 >= points.size()) { + return {}; + } + + const auto p_front = autoware_utils::get_point(points.at(seg_idx)); + const auto p_back = autoware_utils::get_point(points.at(seg_idx + 1)); + + try { + validateNonSharpAngle(p_front, p_target, p_back); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return {}; + } + + const auto overlap_with_front = + autoware_utils::calc_distance2d(p_target, p_front) < overlap_threshold; + const auto overlap_with_back = + autoware_utils::calc_distance2d(p_target, p_back) < overlap_threshold; + + const auto is_driving_forward = isDrivingForward(points); + if (!is_driving_forward) { + return {}; + } + + geometry_msgs::msg::Pose target_pose; + { + const auto p_base = is_driving_forward.value() ? p_back : p_front; + const auto pitch = autoware_utils::calc_elevation_angle(p_target, p_base); + const auto yaw = autoware_utils::calc_azimuth_angle(p_target, p_base); + + target_pose.position = p_target; + target_pose.orientation = autoware_utils::create_quaternion_from_rpy(0.0, pitch, yaw); + } + + auto p_insert = points.at(seg_idx); + autoware_utils::set_pose(target_pose, p_insert); + + geometry_msgs::msg::Pose base_pose; + { + const auto p_base = is_driving_forward.value() ? p_front : p_back; + const auto pitch = autoware_utils::calc_elevation_angle(p_base, p_target); + const auto yaw = autoware_utils::calc_azimuth_angle(p_base, p_target); + + base_pose.position = autoware_utils::get_point(p_base); + base_pose.orientation = autoware_utils::create_quaternion_from_rpy(0.0, pitch, yaw); + } + + if (!overlap_with_front && !overlap_with_back) { + if (is_driving_forward.value()) { + autoware_utils::set_pose(base_pose, points.at(seg_idx)); + } else { + autoware_utils::set_pose(base_pose, points.at(seg_idx + 1)); + } + points.insert(points.begin() + seg_idx + 1, p_insert); + return seg_idx + 1; + } + + if (overlap_with_back) { + return seg_idx + 1; + } + + return seg_idx; +} + +extern template std::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template std::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template std::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); + +/** + * @brief insert a point in points container (trajectory, path, ...) using length of point to be + * inserted + * @param insert_point_length length to insert point from the beginning of the points + * @param p_target point to be inserted + * @param points output points of trajectory, path, ... + * @param overlap_threshold distance threshold, used to check if the inserted point is between start + * and end of nominated segment to be added in. + * @return index of segment id, where point is inserted + */ +template +std::optional insertTargetPoint( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, T & points, + const double overlap_threshold = 1e-3) +{ + validateNonEmpty(points); + + if (insert_point_length < 0.0) { + return std::nullopt; + } + + // Get Nearest segment index + std::optional segment_idx = std::nullopt; + for (size_t i = 1; i < points.size(); ++i) { + // TODO(Mamoru Sobue): find accumulated sum beforehand + const double length = calcSignedArcLength(points, 0, i); + if (insert_point_length <= length) { + segment_idx = i - 1; + break; + } + } + + if (!segment_idx) { + return std::nullopt; + } + + return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); +} + +extern template std::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template std::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template std::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); + +/** + * @brief insert a point in points container (trajectory, path, ...) using segment index and length + * of point to be inserted + * @param src_segment_idx source segment index on the trajectory + * @param insert_point_length length to insert point from the beginning of the points + * @param points output points of trajectory, path, ... + * @param overlap_threshold distance threshold, used to check if the inserted point is between start + * and end of nominated segment to be added in. + * @return index of insert point + */ +template +std::optional insertTargetPoint( + const size_t src_segment_idx, const double insert_point_length, T & points, + const double overlap_threshold = 1e-3) +{ + validateNonEmpty(points); + + if (src_segment_idx >= points.size() - 1) { + return std::nullopt; + } + + // Get Nearest segment index + std::optional segment_idx = std::nullopt; + if (0.0 <= insert_point_length) { + for (size_t i = src_segment_idx + 1; i < points.size(); ++i) { + const double length = calcSignedArcLength(points, src_segment_idx, i); + if (insert_point_length <= length) { + segment_idx = i - 1; + break; + } + } + } else { + for (int i = src_segment_idx - 1; 0 <= i; --i) { + const double length = calcSignedArcLength(points, src_segment_idx, i); + if (length <= insert_point_length) { + segment_idx = i; + break; + } + } + } + + if (!segment_idx) { + return std::nullopt; + } + + // Get Target Point + const double segment_length = calcSignedArcLength(points, *segment_idx, *segment_idx + 1); + const double target_length = + insert_point_length - calcSignedArcLength(points, src_segment_idx, *segment_idx); + const double ratio = std::clamp(target_length / segment_length, 0.0, 1.0); + const auto p_target = autoware_utils::calc_interpolated_point( + autoware_utils::get_point(points.at(*segment_idx)), + autoware_utils::get_point(points.at(*segment_idx + 1)), ratio); + + return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); +} + +extern template std::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template std::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template std::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold = 1e-3); + +/** + * @brief Insert a target point from a source pose on the trajectory + * @param src_pose source pose on the trajectory + * @param insert_point_length length to insert point from the beginning of the points + * @param points output points of trajectory, path, ... + * @param max_dist max distance, used to search for nearest segment index in points container to the + * given source pose + * @param max_yaw max yaw, used to search for nearest segment index in points container to the given + * source pose + * @param overlap_threshold distance threshold, used to check if the inserted point is between start + * and end of nominated segment to be added in. + * @return index of insert point + */ +template +std::optional insertTargetPoint( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, T & points, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3) +{ + validateNonEmpty(points); + + if (insert_point_length < 0.0) { + return std::nullopt; + } + + const auto nearest_segment_idx = findNearestSegmentIndex(points, src_pose, max_dist, max_yaw); + if (!nearest_segment_idx) { + return std::nullopt; + } + + const double offset_length = + calcLongitudinalOffsetToSegment(points, *nearest_segment_idx, src_pose.position); + + return insertTargetPoint( + *nearest_segment_idx, insert_point_length + offset_length, points, overlap_threshold); +} + +extern template std::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); +extern template std::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); +extern template std::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); + +/** + * @brief Insert stop point from the source segment index + * @param src_segment_idx start segment index on the trajectory + * @param distance_to_stop_point distance to stop point from the source index + * @param points_with_twist output points of trajectory, path, ... (with velocity) + * @param overlap_threshold distance threshold, used to check if the inserted point is between start + * and end of nominated segment to be added in. + * @return index of stop point + */ +template +std::optional insertStopPoint( + const size_t src_segment_idx, const double distance_to_stop_point, T & points_with_twist, + const double overlap_threshold = 1e-3) +{ + validateNonEmpty(points_with_twist); + + if (distance_to_stop_point < 0.0 || src_segment_idx >= points_with_twist.size() - 1) { + return std::nullopt; + } + + const auto stop_idx = insertTargetPoint( + src_segment_idx, distance_to_stop_point, points_with_twist, overlap_threshold); + if (!stop_idx) { + return std::nullopt; + } + + for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { + autoware_utils::set_longitudinal_velocity(0.0, points_with_twist.at(i)); + } + + return stop_idx; +} + +extern template std::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +extern template std::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +extern template std::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); + +/** + * @brief Insert stop point from the front point of the path + * @param distance_to_stop_point distance to stop point from the front point of the path + * @param points_with_twist output points of trajectory, path, ... (with velocity) + * @param overlap_threshold distance threshold, used to check if the inserted point is between start + * and end of nominated segment to be added in. + * @return index of stop point + */ +template +std::optional insertStopPoint( + const double distance_to_stop_point, T & points_with_twist, const double overlap_threshold = 1e-3) +{ + validateNonEmpty(points_with_twist); + + if (distance_to_stop_point < 0.0) { + return std::nullopt; + } + + double accumulated_length = 0; + for (size_t i = 0; i < points_with_twist.size() - 1; ++i) { + const auto curr_pose = autoware_utils::get_pose(points_with_twist.at(i)); + const auto next_pose = autoware_utils::get_pose(points_with_twist.at(i + 1)); + const double length = autoware_utils::calc_distance2d(curr_pose, next_pose); + if (accumulated_length + length + overlap_threshold > distance_to_stop_point) { + const double insert_length = distance_to_stop_point - accumulated_length; + return insertStopPoint(i, insert_length, points_with_twist, overlap_threshold); + } + accumulated_length += length; + } + + return std::nullopt; +} + +extern template std::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +extern template std::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +extern template std::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); + +/** + * @brief Insert Stop point from the source pose + * @param src_pose source pose + * @param distance_to_stop_point distance to stop point from the src point + * @param points_with_twist output points of trajectory, path, ... (with velocity) + * @param max_dist max distance, used to search for nearest segment index in points container to the + * given source pose + * @param max_yaw max yaw, used to search for nearest segment index in points container to the given + * source pose + * @param overlap_threshold distance threshold, used to check if the inserted point is between start + * and end of nominated segment to be added in. + * @return index of stop point + */ +template +std::optional insertStopPoint( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + T & points_with_twist, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3) +{ + validateNonEmpty(points_with_twist); + + if (distance_to_stop_point < 0.0) { + return std::nullopt; + } + + const auto stop_idx = insertTargetPoint( + src_pose, distance_to_stop_point, points_with_twist, max_dist, max_yaw, overlap_threshold); + + if (!stop_idx) { + return std::nullopt; + } + + for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { + autoware_utils::set_longitudinal_velocity(0.0, points_with_twist.at(i)); + } + + return stop_idx; +} + +extern template std::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); +extern template std::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); +extern template std::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); + +/** + * @brief Insert Stop point that is in the stop segment index + * @param stop_seg_idx segment index of the stop pose + * @param stop_point stop point + * @param points_with_twist output points of trajectory, path, ... (with velocity) + * @param overlap_threshold distance threshold, used to check if the inserted point is between start + * and end of nominated segment to be added in. + * @return index of stop point + */ +template +std::optional insertStopPoint( + const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, T & points_with_twist, + const double overlap_threshold = 1e-3) +{ + const auto insert_idx = autoware::motion_utils::insertTargetPoint( + stop_seg_idx, stop_point, points_with_twist, overlap_threshold); + + if (!insert_idx) { + return std::nullopt; + } + + for (size_t i = insert_idx.value(); i < points_with_twist.size(); ++i) { + autoware_utils::set_longitudinal_velocity(0.0, points_with_twist.at(i)); + } + + return insert_idx; +} + +extern template std::optional +insertStopPoint>( + const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); + +/** + * @brief Insert deceleration point from the source pose + * @param src_point source point + * @param distance_to_decel_point distance to deceleration point from the src point + * @param velocity velocity of stop point + * @param points_with_twist output points of trajectory, path, ... (with velocity) + */ +template +std::optional insertDecelPoint( + const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, + const double velocity, T & points_with_twist) +{ + const auto decel_point = + calcLongitudinalOffsetPoint(points_with_twist, src_point, distance_to_decel_point); + + if (!decel_point) { + return {}; + } + + const auto seg_idx = findNearestSegmentIndex(points_with_twist, decel_point.value()); + const auto insert_idx = insertTargetPoint(seg_idx, decel_point.value(), points_with_twist); + + if (!insert_idx) { + return {}; + } + + for (size_t i = insert_idx.value(); i < points_with_twist.size(); ++i) { + const auto & original_velocity = + autoware_utils::get_longitudinal_velocity(points_with_twist.at(i)); + autoware_utils::set_longitudinal_velocity( + std::min(original_velocity, velocity), points_with_twist.at(i)); + } + + return insert_idx; +} + +extern template std::optional +insertDecelPoint>( + const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, + const double velocity, + std::vector & points_with_twist); + +/** + * @brief Insert orientation to each point in points container (trajectory, path, ...) + * @param points points of trajectory, path, ... (input / output) + * @param is_driving_forward flag indicating the order of points is forward or backward + */ +template +void insertOrientation(T & points, const bool is_driving_forward) +{ + if (is_driving_forward) { + for (size_t i = 0; i < points.size() - 1; ++i) { + const auto & src_point = autoware_utils::get_point(points.at(i)); + const auto & dst_point = autoware_utils::get_point(points.at(i + 1)); + const double pitch = autoware_utils::calc_elevation_angle(src_point, dst_point); + const double yaw = autoware_utils::calc_azimuth_angle(src_point, dst_point); + autoware_utils::set_orientation( + autoware_utils::create_quaternion_from_rpy(0.0, pitch, yaw), points.at(i)); + if (i == points.size() - 2) { + // Terminal orientation is same as the point before it + autoware_utils::set_orientation( + autoware_utils::get_pose(points.at(i)).orientation, points.at(i + 1)); + } + } + } else { + for (size_t i = points.size() - 1; i >= 1; --i) { + const auto & src_point = autoware_utils::get_point(points.at(i)); + const auto & dst_point = autoware_utils::get_point(points.at(i - 1)); + const double pitch = autoware_utils::calc_elevation_angle(src_point, dst_point); + const double yaw = autoware_utils::calc_azimuth_angle(src_point, dst_point); + autoware_utils::set_orientation( + autoware_utils::create_quaternion_from_rpy(0.0, pitch, yaw), points.at(i)); + } + // Initial orientation is same as the point after it + autoware_utils::set_orientation( + autoware_utils::get_pose(points.at(1)).orientation, points.at(0)); + } +} + +extern template void insertOrientation>( + std::vector & points, const bool is_driving_forward); +extern template void +insertOrientation>( + std::vector & points, + const bool is_driving_forward); +extern template void insertOrientation>( + std::vector & points, + const bool is_driving_forward); + +/** + * @brief Remove points with invalid orientation differences from a given points container + * (trajectory, path, ...). Check the difference between the angles of two points and the difference + * between the azimuth angle between the two points and the angle of the next point. + * @param points Points of trajectory, path, or other point container (input / output) + * @param max_yaw_diff Maximum acceptable yaw angle difference between two consecutive points in + * radians (default: M_PI_2) + */ +template +void removeFirstInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2) +{ + for (auto itr = points.begin(); std::next(itr) != points.end();) { + const auto p1 = autoware_utils::get_pose(*itr); + const auto p2 = autoware_utils::get_pose(*std::next(itr)); + const double yaw1 = tf2::getYaw(p1.orientation); + const double yaw2 = tf2::getYaw(p2.orientation); + + if ( + max_yaw_diff < std::abs(autoware_utils::normalize_radian(yaw1 - yaw2)) || + !autoware_utils::is_driving_forward(p1, p2)) { + points.erase(std::next(itr)); + return; + } else { + itr++; + } + } +} + +/** + * @brief calculate length of 2D distance between two points, specified by start point and end + * point with their segment indices in points container + * @param points points of trajectory, path, ... + * @param src_point start point + * @param src_seg_idx index of start point segment + * @param dst_point end point + * @param dst_seg_idx index of end point segment + * @return length of distance between two points. + * Length is positive if destination point is greater that source point (i.e. after it in + * trajectory, path, ...) and negative otherwise. + */ +template +double calcSignedArcLength( + const T & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx) +{ + validateNonEmpty(points); + + const double signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_seg_idx); + const double signed_length_src_offset = + calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); + const double signed_length_dst_offset = + calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); + + return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; +} + +extern template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); + +/** + * @brief calculate length of 2D distance between two points, specified by start point and its + * segment index in points container and end point index in points container + * @param points points of trajectory, path, ... + * @param src_point start point + * @param src_seg_idx index of start point segment + * @param dst_idx index of end point + * @return length of distance between two points + * Length is positive if destination point associated to dst_idx is greater that source point (i.e. + * after it in trajectory, path, ...) and negative otherwise. + */ +template +double calcSignedArcLength( + const T & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const size_t dst_idx) +{ + validateNonEmpty(points); + + const double signed_length_on_traj = calcSignedArcLength(points, src_seg_idx, dst_idx); + const double signed_length_src_offset = + calcLongitudinalOffsetToSegment(points, src_seg_idx, src_point); + + return signed_length_on_traj - signed_length_src_offset; +} + +extern template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); + +/** + * @brief calculate length of 2D distance between two points, specified by start point index in + * points container and end point and its segment index in points container + * @param points points of trajectory, path, ... + * @param src_idx index of start point start point + * @param dst_point end point + * @param dst_seg_idx index of end point segment + * @return length of distance between two points + * Length is positive if destination point is greater that source point associated to src_idx (i.e. + * after it in trajectory, path, ...) and negative otherwise. + */ +template +double calcSignedArcLength( + const T & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point, + const size_t dst_seg_idx) +{ + validateNonEmpty(points); + + const double signed_length_on_traj = calcSignedArcLength(points, src_idx, dst_seg_idx); + const double signed_length_dst_offset = + calcLongitudinalOffsetToSegment(points, dst_seg_idx, dst_point); + + return signed_length_on_traj + signed_length_dst_offset; +} + +extern template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); + +/** + * @brief find first nearest point index through points container for a given pose with soft + * distance and yaw constraints. Finding nearest point is determined by looping through the points + * container, and finding the nearest point to the given pose in terms of squared 2D distance and + * yaw deviation. The index of the point with minimum distance and yaw deviation comparing to the + * given pose will be returned. + * @param points points of trajectory, path, ... + * @param pose given pose + * @param dist_threshold distance threshold used for searching for first nearest index to given pose + * @param yaw_threshold yaw threshold used for searching for first nearest index to given pose + * @return index of nearest point (index or none if not found) + */ +template +size_t findFirstNearestIndexWithSoftConstraints( + const T & points, const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()) +{ + validateNonEmpty(points); + + { // with dist and yaw thresholds + const double squared_dist_threshold = dist_threshold * dist_threshold; + double min_squared_dist = std::numeric_limits::max(); + size_t min_idx = 0; + bool is_within_constraints = false; + for (size_t i = 0; i < points.size(); ++i) { + const auto squared_dist = + autoware_utils::calc_squared_distance2d(points.at(i), pose.position); + const auto yaw = + autoware_utils::calc_yaw_deviation(autoware_utils::get_pose(points.at(i)), pose); + + if (squared_dist_threshold < squared_dist || yaw_threshold < std::abs(yaw)) { + if (is_within_constraints) { + break; + } + continue; + } + + if (min_squared_dist <= squared_dist) { + continue; + } + + min_squared_dist = squared_dist; + min_idx = i; + is_within_constraints = true; + } + + // nearest index is found + if (is_within_constraints) { + return min_idx; + } + } + + { // with dist threshold + const double squared_dist_threshold = dist_threshold * dist_threshold; + double min_squared_dist = std::numeric_limits::max(); + size_t min_idx = 0; + bool is_within_constraints = false; + for (size_t i = 0; i < points.size(); ++i) { + const auto squared_dist = + autoware_utils::calc_squared_distance2d(points.at(i), pose.position); + + if (squared_dist_threshold < squared_dist) { + if (is_within_constraints) { + break; + } + continue; + } + + if (min_squared_dist <= squared_dist) { + continue; + } + + min_squared_dist = squared_dist; + min_idx = i; + is_within_constraints = true; + } + + // nearest index is found + if (is_within_constraints) { + return min_idx; + } + } + + // without any threshold + return findNearestIndex(points, pose.position); +} + +extern template size_t +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); +extern template size_t findFirstNearestIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); +extern template size_t +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); + +/** + * @brief find nearest segment index to pose with soft constraints + * Segment is straight path between two continuous points of trajectory + * When pose is on a trajectory point whose index is nearest_idx, return nearest_idx - 1 + * @param points points of trajectory, path, .. + * @param pose pose to which to find nearest segment index + * @param dist_threshold distance threshold used for searching for first nearest index to given pose + * @param yaw_threshold yaw threshold used for searching for first nearest index to given pose + * @return nearest index + */ +template +size_t findFirstNearestSegmentIndexWithSoftConstraints( + const T & points, const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()) +{ + // find first nearest index with soft constraints (not segment index) + const size_t nearest_idx = + findFirstNearestIndexWithSoftConstraints(points, pose, dist_threshold, yaw_threshold); + + // calculate segment index + if (nearest_idx == 0) { + return 0; + } + if (nearest_idx == points.size() - 1) { + return points.size() - 2; + } + + const double signed_length = calcLongitudinalOffsetToSegment(points, nearest_idx, pose.position); + + if (signed_length <= 0) { + return nearest_idx - 1; + } + + return nearest_idx; +} + +extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); +extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); +extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); + +/** + * @brief calculate the point offset from source point along the trajectory (or path) + * @brief calculate length of 2D distance between given pose and first point in container with zero + * longitudinal velocity + * @param points_with_twist points of trajectory, path, ... (with velocity) + * @param pose given pose to start the distance calculation from + * @param max_dist max distance, used to search for nearest segment index in points container to the + * given pose + * @param max_yaw max yaw, used to search for nearest segment index in points container to the given + * pose + * @return Length of 2D distance between given pose and first point in container with zero + * longitudinal velocity + */ +template +std::optional calcDistanceToForwardStopPoint( + const T & points_with_twist, const geometry_msgs::msg::Pose & pose, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()) +{ + try { + validateNonEmpty(points_with_twist); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "Failed to calculate stop distance %s", e.what()); + return {}; + } + + const auto nearest_segment_idx = + autoware::motion_utils::findNearestSegmentIndex(points_with_twist, pose, max_dist, max_yaw); + + if (!nearest_segment_idx) { + return std::nullopt; + } + + const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex( + points_with_twist, *nearest_segment_idx + 1, points_with_twist.size()); + + if (!stop_idx) { + return std::nullopt; + } + + const auto closest_stop_dist = + calcSignedArcLength(points_with_twist, pose.position, *nearest_segment_idx, *stop_idx); + + return std::max(0.0, closest_stop_dist); +} + +extern template std::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template std::optional calcDistanceToForwardStopPoint< + std::vector>( + const std::vector & points_with_twist, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template std::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); + +// NOTE: Points after forward length from the point will be cropped +// forward_length is assumed to be positive. +template +T cropForwardPoints( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length) +{ + if (points.empty()) { + return T{}; + } + + double sum_length = + -autoware::motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { + sum_length += autoware_utils::calc_distance2d(points.at(i), points.at(i - 1)); + if (forward_length < sum_length) { + const size_t end_idx = i; + return T{points.begin(), points.begin() + end_idx}; + } + } + + return points; +} + +extern template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); +extern template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); +extern template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); + +// NOTE: Points before backward length from the point will be cropped +// backward_length is assumed to be positive. +template +T cropBackwardPoints( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length) +{ + if (points.empty()) { + return T{}; + } + + double sum_length = + -autoware::motion_utils::calcLongitudinalOffsetToSegment(points, target_seg_idx, target_pos); + for (int i = target_seg_idx; 0 < i; --i) { + sum_length -= autoware_utils::calc_distance2d(points.at(i), points.at(i - 1)); + if (sum_length < -backward_length) { + const size_t begin_idx = i; + return T{points.begin() + begin_idx, points.end()}; + } + } + + return points; +} + +extern template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); +extern template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); +extern template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); + +template +T cropPoints( + const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length) +{ + if (points.empty()) { + return T{}; + } + + // NOTE: Cropping forward must be done first in order to keep target_seg_idx. + const auto cropped_forward_points = + cropForwardPoints(points, target_pos, target_seg_idx, forward_length); + + const size_t modified_target_seg_idx = + std::min(target_seg_idx, cropped_forward_points.size() - 2); + const auto cropped_points = cropBackwardPoints( + cropped_forward_points, target_pos, modified_target_seg_idx, backward_length); + + if (cropped_points.size() < 2) { + RCLCPP_DEBUG(get_logger(), "Return original points since cropped_points size is less than 2."); + return points; + } + + return cropped_points; +} + +extern template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); +extern template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); +extern template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); + +/** + * @brief Calculate the angle of the input pose with respect to the nearest trajectory segment. + * The function gets the nearest segment index between the points of the trajectory and the given + * pose's position, then calculates the azimuth angle of that segment and compares it to the yaw of + * the input pose. The segment is a straight path between two continuous points of the trajectory. + * @param points Points of the trajectory, path, ... + * @param pose Input pose with position and orientation (yaw) + * @param throw_exception Flag to enable/disable exception throwing + * @return Angle with respect to the trajectory segment (signed) in radians + */ +template +double calcYawDeviation( + const T & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false) +{ + const auto overlap_removed_points = removeOverlapPoints(points, 0); + + if (throw_exception) { + validateNonEmpty(overlap_removed_points); + } else { + try { + validateNonEmpty(overlap_removed_points); + } catch (const std::exception & e) { + RCLCPP_DEBUG(get_logger(), "%s", e.what()); + return 0.0; + } + } + + if (overlap_removed_points.size() <= 1) { + const std::string error_message( + "[autoware_motion_utils] " + std::string(__func__) + + " Given points size is less than 2. Failed to calculate yaw deviation."); + autoware_utils::print_backtrace(); + if (throw_exception) { + throw std::runtime_error(error_message); + } + RCLCPP_DEBUG( + get_logger(), + "%s Return 0 since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); + return 0.0; + } + + const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, pose.position); + + const double path_yaw = autoware_utils::calc_azimuth_angle( + autoware_utils::get_point(overlap_removed_points.at(seg_idx)), + autoware_utils::get_point(overlap_removed_points.at(seg_idx + 1))); + const double pose_yaw = tf2::getYaw(pose.orientation); + + return autoware_utils::normalize_radian(pose_yaw - path_yaw); +} + +extern template double calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); +extern template double +calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); +extern template double calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); + +/** + * @brief Check if the given target point is in front of the based pose from the trajectory. + * if the points is empty, the function returns false + * @param points Points of the trajectory, path, ... + * @param base_point Base point + * @param target_point Target point + * @param threshold threshold for judging front point + * @return true if the target pose is in front of the base pose + */ +template +bool isTargetPointFront( + const T & points, const geometry_msgs::msg::Point & base_point, + const geometry_msgs::msg::Point & target_point, const double threshold = 0.0) +{ + if (points.empty()) { + return false; + } + + const double s_base = calcSignedArcLength(points, 0, base_point); + const double s_target = calcSignedArcLength(points, 0, target_point); + + return s_target - s_base > threshold; +} + +extern template bool isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold = 0.0); +extern template bool +isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold = 0.0); +extern template bool isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold = 0.0); + +/// @brief calculate the time_from_start fields of the given trajectory points +/// @details this function assumes constant longitudinal velocity between points +/// @param trajectory trajectory for which to calculate the time_from_start +/// @param current_ego_point current ego position +/// @param min_velocity minimum velocity used for a trajectory point +void calculate_time_from_start( + std::vector & trajectory, + const geometry_msgs::msg::Point & current_ego_point, const float min_velocity = 1.0f); + +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/vehicle/vehicle_state_checker.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/vehicle/vehicle_state_checker.hpp new file mode 100644 index 00000000..f1d5677d --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/vehicle/vehicle_state_checker.hpp @@ -0,0 +1,82 @@ +// 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__MOTION_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ +#define AUTOWARE__MOTION_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ + +#include + +#include +#include +#include + +#include + +namespace autoware::motion_utils +{ + +using autoware_planning_msgs::msg::Trajectory; +using geometry_msgs::msg::TwistStamped; +using nav_msgs::msg::Odometry; + +class VehicleStopCheckerBase +{ +public: + VehicleStopCheckerBase(rclcpp::Node * node, double buffer_duration); + rclcpp::Logger getLogger() { return logger_; } + void addTwist(const TwistStamped & twist); + bool isVehicleStopped(const double stop_duration = 0.0) const; + +protected: + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_; + +private: + double buffer_duration_; + std::deque twist_buffer_; +}; + +class VehicleStopChecker : public VehicleStopCheckerBase +{ +public: + explicit VehicleStopChecker(rclcpp::Node * node); + +protected: + rclcpp::Subscription::SharedPtr sub_odom_; + Odometry::ConstSharedPtr odometry_ptr_; + +private: + static constexpr double velocity_buffer_time_sec = 10.0; + void onOdom(const Odometry::ConstSharedPtr msg); +}; + +class VehicleArrivalChecker : public VehicleStopChecker +{ +public: + explicit VehicleArrivalChecker(rclcpp::Node * node); + + bool isVehicleStoppedAtStopPoint(const double stop_duration = 0.0) const; + +private: + static constexpr double th_arrived_distance_m = 1.0; + + rclcpp::Subscription::SharedPtr sub_trajectory_; + + Trajectory::ConstSharedPtr trajectory_ptr_; + + void onTrajectory(const Trajectory::ConstSharedPtr msg); +}; +} // namespace autoware::motion_utils + +#endif // AUTOWARE__MOTION_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_ diff --git a/common/autoware_motion_utils/media/ego_nearest_search.svg b/common/autoware_motion_utils/media/ego_nearest_search.svg new file mode 100644 index 00000000..fb7d94ec --- /dev/null +++ b/common/autoware_motion_utils/media/ego_nearest_search.svg @@ -0,0 +1,280 @@ + + + + + + + + + + + + + +
+
+
nearest
+
+
+ + nearest + + + + + + + + + +
+
+
+ 2 * yaw +
+ threshold +
+
+
+ + 2 * yaw... + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
nearest
+
+
+ + nearest + + + + + + + + + + + + + + + + + + + + + +
+
+
nearest
+
+
+ + nearest + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
nearest
+
+
+ + nearest + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ distance +
+ threshold +
+
+
+ + distance... + + + + + + + +
+
+
1.
+
+
+ + 1. + + + + + + +
+
+
2.
+
+
+ + 2. + + + + + + +
+
+
3.
+
+
+ + 3. + + + + + + +
+
+
4.
+
+
+ + 4. + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/common/autoware_motion_utils/media/segment.svg b/common/autoware_motion_utils/media/segment.svg new file mode 100644 index 00000000..eae72d6c --- /dev/null +++ b/common/autoware_motion_utils/media/segment.svg @@ -0,0 +1,344 @@ + + + + + + + + + + + + + + + + + + + + +
+
+
+ point index +
+
+
+ + point index + + + + + + + + +
+
+
1
+
+
+ + 1 + + + + + + +
+
+
2
+
+
+ + 2 + + + + + + +
+
+
+ segment index +
+
+
+ + segment index + + + + + + +
+
+
1
+
+
+ + 1 + + + + + + +
+
+
7
+
+
+ + 7 + + + + + + +
+
+
8
+
+
+ + 8 + + + + + + +
+
+
7
+
+
+ + 7 + + + + + + +
+
+
6
+
+
+ + 6 + + + + + + +
+
+
6
+
+
+ + 6 + + + + + + +
+
+
+ nearest +
+ index +
+
+
+ + nearest... + + + + + + +
+
+
+ nearest +
+ segment index +
+
+
+ + nearest... + + + + + + +
+
+
0
+
+
+ + 0 + + + + + + +
+
+
0
+
+
+ + 0 + + + + + + +
+
+
3
+
+
+ + 3 + + + + + + +
+
+
2
+
+
+ + 2 + + + + + + +
+
+
5
+
+
+ + 5 + + + + + + +
+
+
3
+
+
+ + 3 + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + +
+
+
5
+
+
+ + 5 + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + + Viewer does not support full SVG 1.1 + + + diff --git a/common/autoware_motion_utils/package.xml b/common/autoware_motion_utils/package.xml new file mode 100644 index 00000000..8b9797f2 --- /dev/null +++ b/common/autoware_motion_utils/package.xml @@ -0,0 +1,46 @@ + + + + autoware_motion_utils + 0.0.0 + The autoware_motion_utils package + Satoshi Ota + Takayuki Murooka + + Fumiya Watanabe + Kosuke Takeuchi + Taiki Tanaka + Takamasa Horibe + Tomoya Kimura + Mamoru Sobue + Shaojie Song + Apache License 2.0 + + Takayuki Murooka + Satoshi Ota + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_internal_planning_msgs + autoware_interpolation + autoware_planning_msgs + autoware_utils + autoware_vehicle_msgs + builtin_interfaces + geometry_msgs + libboost-dev + rclcpp + tf2 + tf2_geometry_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/autoware_motion_utils/src/distance/distance.cpp b/common/autoware_motion_utils/src/distance/distance.cpp new file mode 100644 index 00000000..c218561b --- /dev/null +++ b/common/autoware_motion_utils/src/distance/distance.cpp @@ -0,0 +1,274 @@ +// 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/motion_utils/distance/distance.hpp" + +#include + +namespace autoware::motion_utils +{ +namespace +{ +bool validCheckDecelPlan( + const double v_end, const double a_end, const double v_target, const double a_target, + const double v_margin, const double a_margin) +{ + const double v_min = v_target - std::abs(v_margin); + const double v_max = v_target + std::abs(v_margin); + const double a_min = a_target - std::abs(a_margin); + const double a_max = a_target + std::abs(a_margin); + + if (v_end < v_min || v_max < v_end) { + return false; + } + if (a_end < a_min || a_max < a_end) { + return false; + } + + return true; +} + +/** + * @brief update traveling distance, velocity and acceleration under constant jerk. + * @param (x) current traveling distance [m/s] + * @param (v) current velocity [m/s] + * @param (a) current acceleration [m/ss] + * @param (j) target jerk [m/sss] + * @param (t) time [s] + * @return updated traveling distance, velocity and acceleration + */ +std::tuple update( + const double x, const double v, const double a, const double j, const double t) +{ + const double a_new = a + j * t; + const double v_new = v + a * t + 0.5 * j * t * t; + const double x_new = x + v * t + 0.5 * a * t * t + (1.0 / 6.0) * j * t * t * t; + + return {x_new, v_new, a_new}; +} + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE: TRAPEZOID + * ACCELERATION PROFILE). this type of profile has ZERO JERK time. + * + * [ACCELERATION PROFILE] + * a ^ + * | + * a0 * + * |* + * ----+-*-------------------*------> t + * | * * + * | * * + * | a1 *************** + * | + * + * [JERK PROFILE] + * j ^ + * | + * | ja **** + * | * + * ----+----***************---------> t + * | * + * | * + * jd ****** + * | + * + * @param (v0) current velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (vt) target velocity [m/s] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @param (t_during_min_acc) duration of constant deceleration [s] + * @return moving distance until velocity is reached vt [m] + */ +std::optional calcDecelDistPlanType1( + const double v0, const double vt, const double a0, const double am, const double ja, + const double jd, const double t_during_min_acc) +{ + constexpr double epsilon = 1e-3; + + // negative jerk time + const double j1 = am < a0 ? jd : ja; + const double t1 = epsilon < (am - a0) / j1 ? (am - a0) / j1 : 0.0; + const auto [x1, v1, a1] = update(0.0, v0, a0, j1, t1); + + // zero jerk time + const double t2 = epsilon < t_during_min_acc ? t_during_min_acc : 0.0; + const auto [x2, v2, a2] = update(x1, v1, a1, 0.0, t2); + + // positive jerk time + const double t3 = epsilon < (0.0 - am) / ja ? (0.0 - am) / ja : 0.0; + const auto [x3, v3, a3] = update(x2, v2, a2, ja, t3); + + const double a_target = 0.0; + const double v_margin = 0.3; // [m/s] + const double a_margin = 0.1; // [m/s^2] + + if (!validCheckDecelPlan(v3, a3, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x3; +} + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE: TRIANGLE + * ACCELERATION PROFILE), This type of profile do NOT have ZERO JERK time. + * + * [ACCELERATION PROFILE] + * a ^ + * | + * a0 * + * |* + * ----+-*-----*--------------------> t + * | * * + * | * * + * | a1 * + * | + * + * [JERK PROFILE] + * j ^ + * | + * | ja **** + * | * + * ----+----*-----------------------> t + * | * + * | * + * jd ****** + * | + * + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + */ +std::optional calcDecelDistPlanType2( + const double v0, const double vt, const double a0, const double ja, const double jd) +{ + constexpr double epsilon = 1e-3; + + const double a1_square = (vt - v0 - 0.5 * (0.0 - a0) / jd * a0) * (2.0 * ja * jd / (ja - jd)); + const double a1 = -std::sqrt(a1_square); + + // negative jerk time + const double t1 = epsilon < (a1 - a0) / jd ? (a1 - a0) / jd : 0.0; + const auto [x1, v1, no_use_a1] = update(0.0, v0, a0, jd, t1); + + // positive jerk time + const double t2 = epsilon < (0.0 - a1) / ja ? (0.0 - a1) / ja : 0.0; + const auto [x2, v2, a2] = update(x1, v1, a1, ja, t2); + + const double a_target = 0.0; + const double v_margin = 0.3; + const double a_margin = 0.1; + + if (!validCheckDecelPlan(v2, a2, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x2; +} + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE: LINEAR ACCELERATION + * PROFILE). This type of profile has only positive jerk time. + * + * [ACCELERATION PROFILE] + * a ^ + * | + * ----+----*-----------------------> t + * | * + * | * + * | * + * |* + * a0 * + * | + * + * [JERK PROFILE] + * j ^ + * | + * ja ****** + * | * + * | * + * ----+----*-----------------------> t + * | + * + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + */ +std::optional calcDecelDistPlanType3( + const double v0, const double vt, const double a0, const double ja) +{ + constexpr double epsilon = 1e-3; + + // positive jerk time + const double t_acc = (0.0 - a0) / ja; + const double t1 = epsilon < t_acc ? t_acc : 0.0; + const auto [x1, v1, a1] = update(0.0, v0, a0, ja, t1); + + const double a_target = 0.0; + const double v_margin = 0.3; + const double a_margin = 0.1; + + if (!validCheckDecelPlan(v1, a1, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x1; +} +} // namespace + +std::optional calcDecelDistWithJerkAndAccConstraints( + const double current_vel, const double target_vel, const double current_acc, const double acc_min, + const double jerk_acc, const double jerk_dec) +{ + if (current_vel < target_vel) { + return {}; + } + + constexpr double epsilon = 1e-3; + const double jerk_before_min_acc = acc_min < current_acc ? jerk_dec : jerk_acc; + const double t_before_min_acc = (acc_min - current_acc) / jerk_before_min_acc; + const double jerk_after_min_acc = jerk_acc; + const double t_after_min_acc = (0.0 - acc_min) / jerk_after_min_acc; + + const double t_during_min_acc = + (target_vel - current_vel - current_acc * t_before_min_acc - + 0.5 * jerk_before_min_acc * std::pow(t_before_min_acc, 2) - acc_min * t_after_min_acc - + 0.5 * jerk_after_min_acc * std::pow(t_after_min_acc, 2)) / + acc_min; + + // check if it is possible to decelerate to the target velocity + // by simply bringing the current acceleration to zero. + const auto is_decel_needed = + 0.5 * (0.0 - current_acc) / jerk_acc * current_acc > target_vel - current_vel; + + if (t_during_min_acc > epsilon) { + return calcDecelDistPlanType1( + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_during_min_acc); + } + if (is_decel_needed || current_acc > epsilon) { + return calcDecelDistPlanType2(current_vel, target_vel, current_acc, jerk_acc, jerk_dec); + } + + return calcDecelDistPlanType3(current_vel, target_vel, current_acc, jerk_acc); +} +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/factor/planing_factor_interface.cpp b/common/autoware_motion_utils/src/factor/planing_factor_interface.cpp new file mode 100644 index 00000000..6c5d687b --- /dev/null +++ b/common/autoware_motion_utils/src/factor/planing_factor_interface.cpp @@ -0,0 +1,49 @@ +// Copyright 2024 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 + +#include +#include + +namespace autoware::motion_utils +{ +template void +PlanningFactorInterface::add( + const std::vector &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, + const std::string &); +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, + const std::string &); + +template void +PlanningFactorInterface::add( + const std::vector &, const Pose &, + const Pose &, const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, + const double, const double, const std::string &); +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/factor/steering_factor_interface.cpp b/common/autoware_motion_utils/src/factor/steering_factor_interface.cpp new file mode 100644 index 00000000..7f12e297 --- /dev/null +++ b/common/autoware_motion_utils/src/factor/steering_factor_interface.cpp @@ -0,0 +1,34 @@ +// 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 + +#include + +namespace autoware::motion_utils +{ +void SteeringFactorInterface::set( + const std::array & pose, const std::array distance, const uint16_t direction, + const uint16_t status, const std::string & detail) +{ + steering_factor_.pose = pose; + std::array converted_distance{0.0, 0.0}; + for (int i = 0; i < 2; ++i) converted_distance[i] = static_cast(distance[i]); + steering_factor_.distance = converted_distance; + steering_factor_.behavior = behavior_; + steering_factor_.direction = direction; + steering_factor_.status = status; + steering_factor_.detail = detail; +} +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp b/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp new file mode 100644 index 00000000..2082ed1e --- /dev/null +++ b/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp @@ -0,0 +1,62 @@ +// Copyright 2023-2024 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 +#include + +#include +#include +#include + +#include +#include + +namespace autoware::motion_utils +{ +template +void VelocityFactorInterface::set( + const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, + const VelocityFactorStatus status, const std::string & detail) +{ + const auto & curr_point = curr_pose.position; + const auto & stop_point = stop_pose.position; + velocity_factor_.behavior = behavior_; + velocity_factor_.pose = stop_pose; + velocity_factor_.distance = + static_cast(autoware::motion_utils::calcSignedArcLength(points, curr_point, stop_point)); + velocity_factor_.status = status; + velocity_factor_.detail = detail; +} + +void VelocityFactorInterface::set( + const double & distance, const VelocityFactorStatus & status, const std::string & detail) +{ + velocity_factor_.behavior = behavior_; + velocity_factor_.distance = static_cast(distance); + velocity_factor_.status = status; + velocity_factor_.detail = detail; +} + +template void +VelocityFactorInterface::set( + const std::vector &, const Pose &, + const Pose &, const VelocityFactorStatus, const std::string &); +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, const Pose &, + const VelocityFactorStatus, const std::string &); +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, const Pose &, + const VelocityFactorStatus, const std::string &); + +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/marker/marker_helper.cpp b/common/autoware_motion_utils/src/marker/marker_helper.cpp new file mode 100644 index 00000000..07dbcd8f --- /dev/null +++ b/common/autoware_motion_utils/src/marker/marker_helper.cpp @@ -0,0 +1,186 @@ +// 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/motion_utils/marker/marker_helper.hpp" + +#include "autoware_utils/ros/marker_helper.hpp" + +#include + +#include + +#include + +using autoware_utils::create_default_marker; +using autoware_utils::create_deleted_default_marker; +using autoware_utils::create_marker_color; +using autoware_utils::create_marker_scale; +using visualization_msgs::msg::MarkerArray; + +namespace +{ + +inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray( + const geometry_msgs::msg::Pose & vehicle_front_pose, const std::string & module_name, + const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id, + const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::MarkerArray marker_array; + + // Virtual Wall + { + auto marker = create_default_marker( + "map", now, ns_prefix + "virtual_wall", id, visualization_msgs::msg::Marker::CUBE, + create_marker_scale(0.1, 5.0, 2.0), color); + + marker.pose = vehicle_front_pose; + marker.pose.position.z += 1.0; + + marker_array.markers.push_back(marker); + } + + // Factor Text + { + auto marker = create_default_marker( + "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + create_marker_scale(0.0, 0.0, 1.0 /*font size*/), create_marker_color(1.0, 1.0, 1.0, 1.0)); + + marker.pose = vehicle_front_pose; + marker.pose.position.z += 2.0; + marker.text = module_name; + + marker_array.markers.push_back(marker); + } + + return marker_array; +} + +inline visualization_msgs::msg::MarkerArray createDeletedVirtualWallMarkerArray( + const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id) +{ + visualization_msgs::msg::MarkerArray marker_array; + + // Virtual Wall + { + auto marker = create_deleted_default_marker(now, ns_prefix + "virtual_wall", id); + marker_array.markers.push_back(marker); + } + + // Factor Text + { + auto marker = create_deleted_default_marker(now, ns_prefix + "factor_text", id); + marker_array.markers.push_back(marker); + } + + return marker_array; +} + +inline visualization_msgs::msg::MarkerArray createIntendedPassArrowMarkerArray( + const geometry_msgs::msg::Pose & vehicle_front_pose, const std::string & module_name, + const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id, + const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::MarkerArray marker_array; + + // Arrow + { + auto marker = create_default_marker( + "map", now, ns_prefix + "direction", id, visualization_msgs::msg::Marker::ARROW, + create_marker_scale(2.5 /*length*/, 0.15 /*width*/, 1.0 /*height*/), color); + + marker.pose = vehicle_front_pose; + + marker_array.markers.push_back(marker); + } + + // Factor Text + { + auto marker = create_default_marker( + "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + create_marker_scale(0.0, 0.0, 0.4 /*font size*/), create_marker_color(1.0, 1.0, 1.0, 1.0)); + + marker.pose = vehicle_front_pose; + marker.pose.position.z += 2.0; + marker.text = module_name; + + marker_array.markers.push_back(marker); + } + + return marker_array; +} + +} // namespace + +namespace autoware::motion_utils +{ +visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) +{ + const auto pose_with_offset = autoware_utils::calc_offset_pose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); + return createVirtualWallMarkerArray( + pose_with_offset, module_name, ns_prefix + "stop_", now, id, + create_marker_color(1.0, 0.0, 0.0, 0.5)); +} + +visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) +{ + const auto pose_with_offset = autoware_utils::calc_offset_pose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); + return createVirtualWallMarkerArray( + pose_with_offset, module_name, ns_prefix + "slow_down_", now, id, + create_marker_color(1.0, 1.0, 0.0, 0.5)); +} + +visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) +{ + const auto pose_with_offset = autoware_utils::calc_offset_pose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); + return createVirtualWallMarkerArray( + pose_with_offset, module_name, ns_prefix + "dead_line_", now, id, + create_marker_color(0.0, 1.0, 0.0, 0.5)); +} + +visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) +{ + const auto pose_with_offset = autoware_utils::calc_offset_pose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); + return createIntendedPassArrowMarkerArray( + pose_with_offset, module_name, ns_prefix + "intended_pass_", now, id, + create_marker_color(0.77, 0.77, 0.77, 0.5)); +} + +visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( + const rclcpp::Time & now, const int32_t id) +{ + return createDeletedVirtualWallMarkerArray("stop_", now, id); +} + +visualization_msgs::msg::MarkerArray createDeletedSlowDownVirtualWallMarker( + const rclcpp::Time & now, const int32_t id) +{ + return createDeletedVirtualWallMarkerArray("slow_down_", now, id); +} +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp new file mode 100644 index 00000000..fb8708b5 --- /dev/null +++ b/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp @@ -0,0 +1,94 @@ +// 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/motion_utils/marker/virtual_wall_marker_creator.hpp" + +#include "autoware/motion_utils/marker/marker_helper.hpp" + +namespace autoware::motion_utils +{ + +void VirtualWallMarkerCreator::cleanup() +{ + for (auto it = marker_count_per_namespace_.begin(); it != marker_count_per_namespace_.end();) { + const auto & marker_count = it->second; + const auto is_unused_namespace = marker_count.previous == 0 && marker_count.current == 0; + if (is_unused_namespace) + it = marker_count_per_namespace_.erase(it); + else + ++it; + } + virtual_walls_.clear(); +} + +void VirtualWallMarkerCreator::add_virtual_wall(const VirtualWall & virtual_wall) +{ + virtual_walls_.push_back(virtual_wall); +} +void VirtualWallMarkerCreator::add_virtual_walls(const VirtualWalls & walls) +{ + virtual_walls_.insert(virtual_walls_.end(), walls.begin(), walls.end()); +} + +visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( + const rclcpp::Time & now) +{ + visualization_msgs::msg::MarkerArray marker_array; + // update marker counts + for (auto & [ns, count] : marker_count_per_namespace_) { + count.previous = count.current; + count.current = 0UL; + } + // convert to markers + create_wall_function create_fn; + for (const auto & virtual_wall : virtual_walls_) { + switch (virtual_wall.style) { + case stop: + create_fn = autoware::motion_utils::createStopVirtualWallMarker; + break; + case slowdown: + create_fn = autoware::motion_utils::createSlowDownVirtualWallMarker; + break; + case deadline: + create_fn = autoware::motion_utils::createDeadLineVirtualWallMarker; + break; + case pass: + create_fn = autoware::motion_utils::createIntendedPassVirtualMarker; + break; + } + const auto wall_text = virtual_wall.detail.empty() + ? virtual_wall.text + : virtual_wall.text + "(" + virtual_wall.detail + ")"; + auto markers = create_fn( + virtual_wall.pose, wall_text, now, 0, virtual_wall.longitudinal_offset, virtual_wall.ns, + virtual_wall.is_driving_forward); + for (auto & marker : markers.markers) { + marker.id = static_cast(marker_count_per_namespace_[marker.ns].current++); + marker_array.markers.push_back(marker); + } + } + // create delete markers + visualization_msgs::msg::Marker marker; + marker.action = visualization_msgs::msg::Marker::DELETE; + for (const auto & [ns, count] : marker_count_per_namespace_) { + for (marker.id = static_cast(count.current); marker.id < static_cast(count.previous); + ++marker.id) { + marker.ns = ns; + marker_array.markers.push_back(marker); + } + } + cleanup(); + return marker_array; +} +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/resample/resample.cpp b/common/autoware_motion_utils/src/resample/resample.cpp new file mode 100644 index 00000000..75126ba1 --- /dev/null +++ b/common/autoware_motion_utils/src/resample/resample.cpp @@ -0,0 +1,752 @@ +// 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/motion_utils/resample/resample.hpp" + +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/zero_order_hold.hpp" +#include "autoware/motion_utils/resample/resample_utils.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware_utils/geometry/geometry.hpp" + +#include +#include +#include +#include + +namespace autoware::motion_utils +{ +std::vector resamplePointVector( + const std::vector & points, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, + const bool use_lerp_for_z) +{ + // validate arguments + if (!resample_utils::validate_arguments(points, resampled_arclength)) { + return points; + } + + // Input Path Information + std::vector input_arclength; + std::vector x; + std::vector y; + std::vector z; + input_arclength.reserve(points.size()); + x.reserve(points.size()); + y.reserve(points.size()); + z.reserve(points.size()); + + input_arclength.push_back(0.0); + x.push_back(points.front().x); + y.push_back(points.front().y); + z.push_back(points.front().z); + for (size_t i = 1; i < points.size(); ++i) { + const auto & prev_pt = points.at(i - 1); + const auto & curr_pt = points.at(i); + const double ds = autoware_utils::calc_distance2d(prev_pt, curr_pt); + input_arclength.push_back(ds + input_arclength.back()); + x.push_back(curr_pt.x); + y.push_back(curr_pt.y); + z.push_back(curr_pt.z); + } + + // Interpolate + const auto lerp = [&](const auto & input) { + return autoware::interpolation::lerp(input_arclength, input, resampled_arclength); + }; + const auto spline = [&](const auto & input) { + return autoware::interpolation::spline(input_arclength, input, resampled_arclength); + }; + const auto spline_by_akima = [&](const auto & input) { + return autoware::interpolation::splineByAkima(input_arclength, input, resampled_arclength); + }; + + const auto interpolated_x = use_akima_spline_for_xy ? lerp(x) : spline_by_akima(x); + const auto interpolated_y = use_akima_spline_for_xy ? lerp(y) : spline_by_akima(y); + const auto interpolated_z = use_lerp_for_z ? lerp(z) : spline(z); + + std::vector resampled_points; + resampled_points.resize(interpolated_x.size()); + + // Insert Position + for (size_t i = 0; i < resampled_points.size(); ++i) { + geometry_msgs::msg::Point point; + point.x = interpolated_x.at(i); + point.y = interpolated_y.at(i); + point.z = interpolated_z.at(i); + resampled_points.at(i) = point; + } + + return resampled_points; +} + +std::vector resamplePointVector( + const std::vector & points, const double resample_interval, + const bool use_akima_spline_for_xy, const bool use_lerp_for_z) +{ + const double input_length = autoware::motion_utils::calcArcLength(points); + + std::vector resampling_arclength; + for (double s = 0.0; s < input_length; s += resample_interval) { + resampling_arclength.push_back(s); + } + if (resampling_arclength.empty()) { + std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; + return points; + } + + // Insert terminal point + if (input_length - resampling_arclength.back() < autoware::motion_utils::overlap_threshold) { + resampling_arclength.back() = input_length; + } else { + resampling_arclength.push_back(input_length); + } + + return resamplePointVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); +} + +std::vector resamplePoseVector( + const std::vector & points_raw, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, + const bool use_lerp_for_z) +{ + // Remove overlap points for resampling + const auto points = autoware::motion_utils::removeOverlapPoints(points_raw); + + // validate arguments + if (!resample_utils::validate_arguments(points, resampled_arclength)) { + return points_raw; + } + + std::vector position(points.size()); + for (size_t i = 0; i < points.size(); ++i) { + position.at(i) = points.at(i).position; + } + const auto resampled_position = + resamplePointVector(position, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); + + std::vector resampled_points(resampled_position.size()); + + // Insert Position + for (size_t i = 0; i < resampled_position.size(); ++i) { + geometry_msgs::msg::Pose pose; + pose.position.x = resampled_position.at(i).x; + pose.position.y = resampled_position.at(i).y; + pose.position.z = resampled_position.at(i).z; + resampled_points.at(i) = pose; + } + + const bool is_driving_forward = autoware_utils::is_driving_forward(points.at(0), points.at(1)); + autoware::motion_utils::insertOrientation(resampled_points, is_driving_forward); + + // Initial orientation is depend on the initial value of the resampled_arclength + // when backward driving + if (!is_driving_forward && resampled_arclength.front() < 1e-3) { + resampled_points.at(0).orientation = points.at(0).orientation; + } + + return resampled_points; +} + +std::vector resamplePoseVector( + const std::vector & points, const double resample_interval, + const bool use_akima_spline_for_xy, const bool use_lerp_for_z) +{ + const double input_length = autoware::motion_utils::calcArcLength(points); + + std::vector resampling_arclength; + for (double s = 0.0; s < input_length; s += resample_interval) { + resampling_arclength.push_back(s); + } + if (resampling_arclength.empty()) { + std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; + return points; + } + + // Insert terminal point + if (input_length - resampling_arclength.back() < autoware::motion_utils::overlap_threshold) { + resampling_arclength.back() = input_length; + } else { + resampling_arclength.push_back(input_length); + } + + return resamplePoseVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); +} + +autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, + const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) +{ + auto resampling_arclength = resampled_arclength; + + // Add resampling_arclength to insert input points which have multiple lane_ids + for (size_t i = 0; i < input_path.points.size(); ++i) { + if (input_path.points.at(i).lane_ids.size() < 2) { + continue; + } + + const double distance_to_resampling_point = calcSignedArcLength(input_path.points, 0, i); + for (size_t j = 1; j < resampling_arclength.size(); ++j) { + if ( + resampling_arclength.at(j - 1) <= distance_to_resampling_point && + distance_to_resampling_point < resampling_arclength.at(j)) { + const double dist_to_prev_point = + std::fabs(distance_to_resampling_point - resampling_arclength.at(j - 1)); + const double dist_to_following_point = + std::fabs(resampling_arclength.at(j) - distance_to_resampling_point); + if (dist_to_prev_point < autoware::motion_utils::overlap_threshold) { + resampling_arclength.at(j - 1) = distance_to_resampling_point; + } else if (dist_to_following_point < autoware::motion_utils::overlap_threshold) { + resampling_arclength.at(j) = distance_to_resampling_point; + } else { + resampling_arclength.insert( + resampling_arclength.begin() + j, distance_to_resampling_point); + } + break; + } + } + } + + // validate arguments + if (!resample_utils::validate_arguments(input_path.points, resampling_arclength)) { + return input_path; + } + + // For LaneIds, is_final + // + // ------|----|----|----|----|----|----|-------> resampled + // [0] [1] [2] [3] [4] [5] [6] + // + // ------|----------------|----------|---------> base + // [0] [1] [2] + // + // resampled[0~3] = base[0] + // resampled[4~5] = base[1] + // resampled[6] = base[2] + + // Input Path Information + std::vector input_arclength; + std::vector input_pose; + std::vector v_lon; + std::vector v_lat; + std::vector heading_rate; + std::vector is_final; + std::vector> lane_ids; + input_arclength.reserve(input_path.points.size()); + input_pose.reserve(input_path.points.size()); + v_lon.reserve(input_path.points.size()); + v_lat.reserve(input_path.points.size()); + heading_rate.reserve(input_path.points.size()); + is_final.reserve(input_path.points.size()); + lane_ids.reserve(input_path.points.size()); + + input_arclength.push_back(0.0); + input_pose.push_back(input_path.points.front().point.pose); + v_lon.push_back(input_path.points.front().point.longitudinal_velocity_mps); + v_lat.push_back(input_path.points.front().point.lateral_velocity_mps); + heading_rate.push_back(input_path.points.front().point.heading_rate_rps); + is_final.push_back(input_path.points.front().point.is_final); + lane_ids.push_back(input_path.points.front().lane_ids); + for (size_t i = 1; i < input_path.points.size(); ++i) { + const auto & prev_pt = input_path.points.at(i - 1).point; + const auto & curr_pt = input_path.points.at(i).point; + const double ds = autoware_utils::calc_distance2d(prev_pt.pose.position, curr_pt.pose.position); + input_arclength.push_back(ds + input_arclength.back()); + input_pose.push_back(curr_pt.pose); + v_lon.push_back(curr_pt.longitudinal_velocity_mps); + v_lat.push_back(curr_pt.lateral_velocity_mps); + heading_rate.push_back(curr_pt.heading_rate_rps); + is_final.push_back(curr_pt.is_final); + lane_ids.push_back(input_path.points.at(i).lane_ids); + } + + if (input_arclength.back() < resampling_arclength.back()) { + std::cerr << "[autoware_motion_utils]: resampled path length is longer than input path length" + << std::endl; + return input_path; + } + + // Interpolate + const auto lerp = [&](const auto & input) { + return autoware::interpolation::lerp(input_arclength, input, resampling_arclength); + }; + + auto closest_segment_indices = + autoware::interpolation::calc_closest_segment_indices(input_arclength, resampling_arclength); + + const auto zoh = [&](const auto & input) { + return autoware::interpolation::zero_order_hold( + input_arclength, input, closest_segment_indices); + }; + + const auto interpolated_pose = + resamplePoseVector(input_pose, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); + const auto interpolated_v_lon = use_zero_order_hold_for_v ? zoh(v_lon) : lerp(v_lon); + const auto interpolated_v_lat = use_zero_order_hold_for_v ? zoh(v_lat) : lerp(v_lat); + const auto interpolated_heading_rate = lerp(heading_rate); + const auto interpolated_is_final = zoh(is_final); + + // interpolate lane_ids + std::vector> interpolated_lane_ids; + interpolated_lane_ids.resize(resampling_arclength.size()); + constexpr double epsilon = 1e-6; + for (size_t i = 0; i < resampling_arclength.size(); ++i) { + const size_t seg_idx = std::min(closest_segment_indices.at(i), input_path.points.size() - 2); + const auto & prev_lane_ids = input_path.points.at(seg_idx).lane_ids; + const auto & next_lane_ids = input_path.points.at(seg_idx + 1).lane_ids; + + if (std::abs(input_arclength.at(seg_idx) - resampling_arclength.at(i)) <= epsilon) { + interpolated_lane_ids.at(i).insert( + interpolated_lane_ids.at(i).end(), prev_lane_ids.begin(), prev_lane_ids.end()); + } else if (std::abs(input_arclength.at(seg_idx + 1) - resampling_arclength.at(i)) <= epsilon) { + interpolated_lane_ids.at(i).insert( + interpolated_lane_ids.at(i).end(), next_lane_ids.begin(), next_lane_ids.end()); + } else { + // extract lane_ids those prev_lane_ids and next_lane_ids have in common + for (const auto target_lane_id : prev_lane_ids) { + if ( + std::find(next_lane_ids.begin(), next_lane_ids.end(), target_lane_id) != + next_lane_ids.end()) { + interpolated_lane_ids.at(i).push_back(target_lane_id); + } + } + // If there are no common lane_ids, the prev_lane_ids is assigned. + if (interpolated_lane_ids.at(i).empty()) { + interpolated_lane_ids.at(i).insert( + interpolated_lane_ids.at(i).end(), prev_lane_ids.begin(), prev_lane_ids.end()); + } + } + } + + if (interpolated_pose.size() != resampling_arclength.size()) { + std::cerr + << "[autoware_motion_utils]: Resampled pose size is different from resampled arclength" + << std::endl; + return input_path; + } + + autoware_internal_planning_msgs::msg::PathWithLaneId resampled_path; + resampled_path.header = input_path.header; + resampled_path.left_bound = input_path.left_bound; + resampled_path.right_bound = input_path.right_bound; + resampled_path.points.resize(interpolated_pose.size()); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + autoware_planning_msgs::msg::PathPoint path_point; + path_point.pose = interpolated_pose.at(i); + path_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); + path_point.lateral_velocity_mps = interpolated_v_lat.at(i); + path_point.heading_rate_rps = interpolated_heading_rate.at(i); + path_point.is_final = interpolated_is_final.at(i); + resampled_path.points.at(i).point = path_point; + resampled_path.points.at(i).lane_ids = interpolated_lane_ids.at(i); + } + + return resampled_path; +} + +autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath( + const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, + const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, + const bool use_zero_order_hold_for_v, const bool resample_input_path_stop_point) +{ + // validate arguments + if (!resample_utils::validate_arguments(input_path.points, resample_interval)) { + return input_path; + } + + // transform input_path + std::vector transformed_input_path( + input_path.points.size()); + for (size_t i = 0; i < input_path.points.size(); ++i) { + transformed_input_path.at(i) = input_path.points.at(i).point; + } + // compute path length + const double input_path_len = autoware::motion_utils::calcArcLength(transformed_input_path); + + std::vector resampling_arclength; + for (double s = 0.0; s < input_path_len; s += resample_interval) { + resampling_arclength.push_back(s); + } + if (resampling_arclength.empty()) { + std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; + return input_path; + } + + // Insert terminal point + if (input_path_len - resampling_arclength.back() < autoware::motion_utils::overlap_threshold) { + resampling_arclength.back() = input_path_len; + } else { + resampling_arclength.push_back(input_path_len); + } + + // Insert stop point + if (resample_input_path_stop_point) { + const auto distance_to_stop_point = + autoware::motion_utils::calcDistanceToForwardStopPoint(transformed_input_path, 0); + if (distance_to_stop_point && !resampling_arclength.empty()) { + for (size_t i = 1; i < resampling_arclength.size(); ++i) { + if ( + resampling_arclength.at(i - 1) <= *distance_to_stop_point && + *distance_to_stop_point < resampling_arclength.at(i)) { + const double dist_to_prev_point = + std::fabs(*distance_to_stop_point - resampling_arclength.at(i - 1)); + const double dist_to_following_point = + std::fabs(resampling_arclength.at(i) - *distance_to_stop_point); + if (dist_to_prev_point < autoware::motion_utils::overlap_threshold) { + resampling_arclength.at(i - 1) = *distance_to_stop_point; + } else if (dist_to_following_point < autoware::motion_utils::overlap_threshold) { + resampling_arclength.at(i) = *distance_to_stop_point; + } else { + resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point); + } + break; + } + } + } + } + + return resamplePath( + input_path, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, + use_zero_order_hold_for_v); +} + +autoware_planning_msgs::msg::Path resamplePath( + const autoware_planning_msgs::msg::Path & input_path, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, + const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) +{ + // validate arguments + if (!resample_utils::validate_arguments(input_path.points, resampled_arclength)) { + return input_path; + } + + // Input Path Information + std::vector input_arclength; + std::vector input_pose; + std::vector v_lon; + std::vector v_lat; + std::vector heading_rate; + input_arclength.reserve(input_path.points.size()); + input_pose.reserve(input_path.points.size()); + v_lon.reserve(input_path.points.size()); + v_lat.reserve(input_path.points.size()); + heading_rate.reserve(input_path.points.size()); + + input_arclength.push_back(0.0); + input_pose.push_back(input_path.points.front().pose); + v_lon.push_back(input_path.points.front().longitudinal_velocity_mps); + v_lat.push_back(input_path.points.front().lateral_velocity_mps); + heading_rate.push_back(input_path.points.front().heading_rate_rps); + for (size_t i = 1; i < input_path.points.size(); ++i) { + const auto & prev_pt = input_path.points.at(i - 1); + const auto & curr_pt = input_path.points.at(i); + const double ds = autoware_utils::calc_distance2d(prev_pt.pose.position, curr_pt.pose.position); + input_arclength.push_back(ds + input_arclength.back()); + input_pose.push_back(curr_pt.pose); + v_lon.push_back(curr_pt.longitudinal_velocity_mps); + v_lat.push_back(curr_pt.lateral_velocity_mps); + heading_rate.push_back(curr_pt.heading_rate_rps); + } + + // Interpolate + const auto lerp = [&](const auto & input) { + return autoware::interpolation::lerp(input_arclength, input, resampled_arclength); + }; + + std::vector closest_segment_indices; + if (use_zero_order_hold_for_v) { + closest_segment_indices = + autoware::interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); + } + const auto zoh = [&](const auto & input) { + return autoware::interpolation::zero_order_hold( + input_arclength, input, closest_segment_indices); + }; + + const auto interpolated_pose = + resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); + const auto interpolated_v_lon = use_zero_order_hold_for_v ? zoh(v_lon) : lerp(v_lon); + const auto interpolated_v_lat = use_zero_order_hold_for_v ? zoh(v_lat) : lerp(v_lat); + const auto interpolated_heading_rate = lerp(heading_rate); + + if (interpolated_pose.size() != resampled_arclength.size()) { + std::cerr + << "[autoware_motion_utils]: Resampled pose size is different from resampled arclength" + << std::endl; + return input_path; + } + + autoware_planning_msgs::msg::Path resampled_path; + resampled_path.header = input_path.header; + resampled_path.left_bound = input_path.left_bound; + resampled_path.right_bound = input_path.right_bound; + resampled_path.points.resize(interpolated_pose.size()); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + autoware_planning_msgs::msg::PathPoint path_point; + path_point.pose = interpolated_pose.at(i); + path_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); + path_point.lateral_velocity_mps = interpolated_v_lat.at(i); + path_point.heading_rate_rps = interpolated_heading_rate.at(i); + resampled_path.points.at(i) = path_point; + } + + return resampled_path; +} + +autoware_planning_msgs::msg::Path resamplePath( + const autoware_planning_msgs::msg::Path & input_path, const double resample_interval, + const bool use_akima_spline_for_xy, const bool use_lerp_for_z, + const bool use_zero_order_hold_for_twist, const bool resample_input_path_stop_point) +{ + // validate arguments + if (!resample_utils::validate_arguments(input_path.points, resample_interval)) { + return input_path; + } + + const double input_path_len = autoware::motion_utils::calcArcLength(input_path.points); + + std::vector resampling_arclength; + for (double s = 0.0; s < input_path_len; s += resample_interval) { + resampling_arclength.push_back(s); + } + if (resampling_arclength.empty()) { + std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; + return input_path; + } + + // Insert terminal point + if (input_path_len - resampling_arclength.back() < autoware::motion_utils::overlap_threshold) { + resampling_arclength.back() = input_path_len; + } else { + resampling_arclength.push_back(input_path_len); + } + + // Insert stop point + if (resample_input_path_stop_point) { + const auto distance_to_stop_point = + autoware::motion_utils::calcDistanceToForwardStopPoint(input_path.points, 0); + if (distance_to_stop_point && !resampling_arclength.empty()) { + for (size_t i = 1; i < resampling_arclength.size(); ++i) { + if ( + resampling_arclength.at(i - 1) <= *distance_to_stop_point && + *distance_to_stop_point < resampling_arclength.at(i)) { + const double dist_to_prev_point = + std::fabs(*distance_to_stop_point - resampling_arclength.at(i - 1)); + const double dist_to_following_point = + std::fabs(resampling_arclength.at(i) - *distance_to_stop_point); + if (dist_to_prev_point < autoware::motion_utils::overlap_threshold) { + resampling_arclength.at(i - 1) = *distance_to_stop_point; + } else if (dist_to_following_point < autoware::motion_utils::overlap_threshold) { + resampling_arclength.at(i) = *distance_to_stop_point; + } else { + resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point); + } + break; + } + } + } + } + + return resamplePath( + input_path, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, + use_zero_order_hold_for_twist); +} + +autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, + const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist) +{ + // validate arguments + if (!resample_utils::validate_arguments(input_trajectory.points, resampled_arclength)) { + return input_trajectory; + } + + // Input Trajectory Information + std::vector input_arclength; + std::vector input_pose; + std::vector v_lon; + std::vector v_lat; + std::vector heading_rate; + std::vector acceleration; + std::vector front_wheel_angle; + std::vector rear_wheel_angle; + std::vector time_from_start; + input_arclength.reserve(input_trajectory.points.size()); + input_pose.reserve(input_trajectory.points.size()); + v_lon.reserve(input_trajectory.points.size()); + v_lat.reserve(input_trajectory.points.size()); + heading_rate.reserve(input_trajectory.points.size()); + acceleration.reserve(input_trajectory.points.size()); + front_wheel_angle.reserve(input_trajectory.points.size()); + rear_wheel_angle.reserve(input_trajectory.points.size()); + time_from_start.reserve(input_trajectory.points.size()); + + input_arclength.push_back(0.0); + input_pose.push_back(input_trajectory.points.front().pose); + v_lon.push_back(input_trajectory.points.front().longitudinal_velocity_mps); + v_lat.push_back(input_trajectory.points.front().lateral_velocity_mps); + heading_rate.push_back(input_trajectory.points.front().heading_rate_rps); + acceleration.push_back(input_trajectory.points.front().acceleration_mps2); + front_wheel_angle.push_back(input_trajectory.points.front().front_wheel_angle_rad); + rear_wheel_angle.push_back(input_trajectory.points.front().rear_wheel_angle_rad); + time_from_start.push_back( + rclcpp::Duration(input_trajectory.points.front().time_from_start).seconds()); + + for (size_t i = 1; i < input_trajectory.points.size(); ++i) { + const auto & prev_pt = input_trajectory.points.at(i - 1); + const auto & curr_pt = input_trajectory.points.at(i); + const double ds = autoware_utils::calc_distance2d(prev_pt.pose.position, curr_pt.pose.position); + + input_arclength.push_back(ds + input_arclength.back()); + input_pose.push_back(curr_pt.pose); + v_lon.push_back(curr_pt.longitudinal_velocity_mps); + v_lat.push_back(curr_pt.lateral_velocity_mps); + heading_rate.push_back(curr_pt.heading_rate_rps); + acceleration.push_back(curr_pt.acceleration_mps2); + front_wheel_angle.push_back(curr_pt.front_wheel_angle_rad); + rear_wheel_angle.push_back(curr_pt.rear_wheel_angle_rad); + time_from_start.push_back(rclcpp::Duration(curr_pt.time_from_start).seconds()); + } + + // Set Zero Velocity After Stop Point + // If the longitudinal velocity is zero, set the velocity to zero after that point. + bool stop_point_found_in_v_lon = false; + constexpr double epsilon = 1e-4; + for (size_t i = 0; i < v_lon.size(); ++i) { + if (std::abs(v_lon.at(i)) < epsilon) { + stop_point_found_in_v_lon = true; + } + if (stop_point_found_in_v_lon) { + v_lon.at(i) = 0.0; + } + } + + // Interpolate + const auto lerp = [&](const auto & input) { + return autoware::interpolation::lerp(input_arclength, input, resampled_arclength); + }; + + std::vector closest_segment_indices; + if (use_zero_order_hold_for_twist) { + closest_segment_indices = + autoware::interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); + } + const auto zoh = [&](const auto & input) { + return autoware::interpolation::zero_order_hold( + input_arclength, input, closest_segment_indices); + }; + + const auto interpolated_pose = + resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); + const auto interpolated_v_lon = use_zero_order_hold_for_twist ? zoh(v_lon) : lerp(v_lon); + const auto interpolated_v_lat = use_zero_order_hold_for_twist ? zoh(v_lat) : lerp(v_lat); + const auto interpolated_heading_rate = lerp(heading_rate); + const auto interpolated_acceleration = + use_zero_order_hold_for_twist ? zoh(acceleration) : lerp(acceleration); + const auto interpolated_front_wheel_angle = lerp(front_wheel_angle); + const auto interpolated_rear_wheel_angle = lerp(rear_wheel_angle); + const auto interpolated_time_from_start = lerp(time_from_start); + + if (interpolated_pose.size() != resampled_arclength.size()) { + std::cerr + << "[autoware_motion_utils]: Resampled pose size is different from resampled arclength" + << std::endl; + return input_trajectory; + } + + autoware_planning_msgs::msg::Trajectory resampled_trajectory; + resampled_trajectory.header = input_trajectory.header; + resampled_trajectory.points.resize(interpolated_pose.size()); + for (size_t i = 0; i < resampled_trajectory.points.size(); ++i) { + autoware_planning_msgs::msg::TrajectoryPoint traj_point; + traj_point.pose = interpolated_pose.at(i); + traj_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); + traj_point.lateral_velocity_mps = interpolated_v_lat.at(i); + traj_point.heading_rate_rps = interpolated_heading_rate.at(i); + traj_point.acceleration_mps2 = interpolated_acceleration.at(i); + traj_point.front_wheel_angle_rad = interpolated_front_wheel_angle.at(i); + traj_point.rear_wheel_angle_rad = interpolated_rear_wheel_angle.at(i); + traj_point.time_from_start = rclcpp::Duration::from_seconds(interpolated_time_from_start.at(i)); + resampled_trajectory.points.at(i) = traj_point; + } + + return resampled_trajectory; +} + +autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double resample_interval, + const bool use_akima_spline_for_xy, const bool use_lerp_for_z, + const bool use_zero_order_hold_for_twist, const bool resample_input_trajectory_stop_point) +{ + // validate arguments + if (!resample_utils::validate_arguments(input_trajectory.points, resample_interval)) { + return input_trajectory; + } + + const double input_trajectory_len = + autoware::motion_utils::calcArcLength(input_trajectory.points); + + std::vector resampling_arclength; + for (double s = 0.0; s < input_trajectory_len; s += resample_interval) { + resampling_arclength.push_back(s); + } + if (resampling_arclength.empty()) { + std::cerr << "[autoware_motion_utils]: resampling arclength is empty" << std::endl; + return input_trajectory; + } + + // Insert terminal point + if ( + input_trajectory_len - resampling_arclength.back() < + autoware::motion_utils::overlap_threshold) { + resampling_arclength.back() = input_trajectory_len; + } else { + resampling_arclength.push_back(input_trajectory_len); + } + + // Insert stop point + if (resample_input_trajectory_stop_point) { + const auto distance_to_stop_point = + autoware::motion_utils::calcDistanceToForwardStopPoint(input_trajectory.points, 0); + if (distance_to_stop_point && !resampling_arclength.empty()) { + for (size_t i = 1; i < resampling_arclength.size(); ++i) { + if ( + resampling_arclength.at(i - 1) <= *distance_to_stop_point && + *distance_to_stop_point < resampling_arclength.at(i)) { + const double dist_to_prev_point = + std::fabs(*distance_to_stop_point - resampling_arclength.at(i - 1)); + const double dist_to_following_point = + std::fabs(resampling_arclength.at(i) - *distance_to_stop_point); + if (dist_to_prev_point < autoware::motion_utils::overlap_threshold) { + resampling_arclength.at(i - 1) = *distance_to_stop_point; + } else if (dist_to_following_point < autoware::motion_utils::overlap_threshold) { + resampling_arclength.at(i) = *distance_to_stop_point; + } else { + resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point); + } + break; + } + } + } + } + + return resampleTrajectory( + input_trajectory, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, + use_zero_order_hold_for_twist); +} + +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/trajectory/conversion.cpp b/common/autoware_motion_utils/src/trajectory/conversion.cpp new file mode 100644 index 00000000..706dd6da --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory/conversion.cpp @@ -0,0 +1,55 @@ +// 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/motion_utils/trajectory/conversion.hpp" + +#include +#include + +namespace autoware::motion_utils +{ +/** + * @brief Convert std::vector to + * autoware_planning_msgs::msg::Trajectory. This function is temporarily added for porting to + * autoware_msgs. We should consider whether to remove this function after the porting is done. + * @attention This function just clips + * std::vector up to the capacity of Trajectory. + * Therefore, the error handling out of this function is necessary if the size of the input greater + * than the capacity. + * @todo Decide how to handle the situation that we need to use the trajectory with the size of + * points larger than the capacity. (Tier IV) + */ +autoware_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory, + const std_msgs::msg::Header & header) +{ + autoware_planning_msgs::msg::Trajectory output{}; + output.header = header; + for (const auto & pt : trajectory) output.points.push_back(pt); + return output; +} + +/** + * @brief Convert autoware_planning_msgs::msg::Trajectory to + * std::vector. + */ +std::vector convertToTrajectoryPointArray( + const autoware_planning_msgs::msg::Trajectory & trajectory) +{ + std::vector output(trajectory.points.size()); + std::copy(trajectory.points.begin(), trajectory.points.end(), output.begin()); + return output; +} + +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/trajectory/interpolation.cpp b/common/autoware_motion_utils/src/trajectory/interpolation.cpp new file mode 100644 index 00000000..e94b9c24 --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory/interpolation.cpp @@ -0,0 +1,149 @@ +// 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/motion_utils/trajectory/interpolation.hpp" + +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" + +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; + +namespace autoware::motion_utils +{ +TrajectoryPoint calcInterpolatedPoint( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose, + const bool use_zero_order_hold_for_twist, const double dist_threshold, const double yaw_threshold) +{ + if (trajectory.points.empty()) { + TrajectoryPoint interpolated_point{}; + interpolated_point.pose = target_pose; + return interpolated_point; + } + if (trajectory.points.size() == 1) { + return trajectory.points.front(); + } + + const size_t segment_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory.points, target_pose, dist_threshold, yaw_threshold); + + // Calculate interpolation ratio + const auto & curr_pt = trajectory.points.at(segment_idx); + const auto & next_pt = trajectory.points.at(segment_idx + 1); + const auto v1 = autoware_utils::point_2_tf_vector(curr_pt, next_pt); + const auto v2 = autoware_utils::point_2_tf_vector(curr_pt, target_pose); + if (v1.length2() < 1e-3) { + return curr_pt; + } + + const double ratio = v1.dot(v2) / v1.length2(); + const double clamped_ratio = std::clamp(ratio, 0.0, 1.0); + + // Interpolate + TrajectoryPoint interpolated_point{}; + + // pose interpolation + interpolated_point.pose = autoware_utils::calc_interpolated_pose(curr_pt, next_pt, clamped_ratio); + + // twist interpolation + if (use_zero_order_hold_for_twist) { + interpolated_point.longitudinal_velocity_mps = curr_pt.longitudinal_velocity_mps; + interpolated_point.lateral_velocity_mps = curr_pt.lateral_velocity_mps; + interpolated_point.acceleration_mps2 = curr_pt.acceleration_mps2; + } else { + interpolated_point.longitudinal_velocity_mps = autoware::interpolation::lerp( + curr_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, clamped_ratio); + interpolated_point.lateral_velocity_mps = autoware::interpolation::lerp( + curr_pt.lateral_velocity_mps, next_pt.lateral_velocity_mps, clamped_ratio); + interpolated_point.acceleration_mps2 = autoware::interpolation::lerp( + curr_pt.acceleration_mps2, next_pt.acceleration_mps2, clamped_ratio); + } + + // heading rate interpolation + interpolated_point.heading_rate_rps = autoware::interpolation::lerp( + curr_pt.heading_rate_rps, next_pt.heading_rate_rps, clamped_ratio); + + // wheel interpolation + interpolated_point.front_wheel_angle_rad = autoware::interpolation::lerp( + curr_pt.front_wheel_angle_rad, next_pt.front_wheel_angle_rad, clamped_ratio); + interpolated_point.rear_wheel_angle_rad = autoware::interpolation::lerp( + curr_pt.rear_wheel_angle_rad, next_pt.rear_wheel_angle_rad, clamped_ratio); + + // time interpolation + const double interpolated_time = autoware::interpolation::lerp( + rclcpp::Duration(curr_pt.time_from_start).seconds(), + rclcpp::Duration(next_pt.time_from_start).seconds(), clamped_ratio); + interpolated_point.time_from_start = rclcpp::Duration::from_seconds(interpolated_time); + + return interpolated_point; +} + +PathPointWithLaneId calcInterpolatedPoint( + const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, + const bool use_zero_order_hold_for_twist, const double dist_threshold, const double yaw_threshold) +{ + if (path.points.empty()) { + PathPointWithLaneId interpolated_point{}; + interpolated_point.point.pose = target_pose; + return interpolated_point; + } + if (path.points.size() == 1) { + return path.points.front(); + } + + const size_t segment_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, target_pose, dist_threshold, yaw_threshold); + + // Calculate interpolation ratio + const auto & curr_pt = path.points.at(segment_idx); + const auto & next_pt = path.points.at(segment_idx + 1); + const auto v1 = autoware_utils::point_2_tf_vector(curr_pt.point, next_pt.point); + const auto v2 = autoware_utils::point_2_tf_vector(curr_pt.point, target_pose); + if (v1.length2() < 1e-3) { + return curr_pt; + } + + const double ratio = v1.dot(v2) / v1.length2(); + const double clamped_ratio = std::clamp(ratio, 0.0, 1.0); + + // Interpolate + PathPointWithLaneId interpolated_point{}; + + // pose interpolation + interpolated_point.point.pose = + autoware_utils::calc_interpolated_pose(curr_pt.point, next_pt.point, clamped_ratio); + + // twist interpolation + if (use_zero_order_hold_for_twist) { + interpolated_point.point.longitudinal_velocity_mps = curr_pt.point.longitudinal_velocity_mps; + interpolated_point.point.lateral_velocity_mps = curr_pt.point.lateral_velocity_mps; + } else { + interpolated_point.point.longitudinal_velocity_mps = autoware::interpolation::lerp( + curr_pt.point.longitudinal_velocity_mps, next_pt.point.longitudinal_velocity_mps, + clamped_ratio); + interpolated_point.point.lateral_velocity_mps = autoware::interpolation::lerp( + curr_pt.point.lateral_velocity_mps, next_pt.point.lateral_velocity_mps, clamped_ratio); + } + + // heading rate interpolation + interpolated_point.point.heading_rate_rps = autoware::interpolation::lerp( + curr_pt.point.heading_rate_rps, next_pt.point.heading_rate_rps, clamped_ratio); + + return interpolated_point; +} +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/trajectory/path_shift.cpp b/common/autoware_motion_utils/src/trajectory/path_shift.cpp new file mode 100644 index 00000000..8864128f --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory/path_shift.cpp @@ -0,0 +1,108 @@ +// Copyright 2024 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/motion_utils/trajectory/path_shift.hpp" + +#include "autoware/motion_utils/trajectory/trajectory.hpp" + +#include +#include + +namespace autoware::motion_utils +{ +double calc_feasible_velocity_from_jerk( + const double lateral, const double jerk, const double longitudinal_distance) +{ + const double j = std::abs(jerk); + const double l = std::abs(lateral); + const double d = std::abs(longitudinal_distance); + if (j < 1.0e-8 || l < 1.0e-8) { + const std::string error_message( + std::string(__func__) + ": Failed to calculate velocity due to invalid arg"); + RCLCPP_WARN(get_logger(), "%s", error_message.c_str()); + return 1.0e10; + } + return d / (4.0 * std::pow(0.5 * l / j, 1.0 / 3.0)); +} + +double calc_lateral_dist_from_jerk( + const double longitudinal, const double jerk, const double velocity) +{ + const double j = std::abs(jerk); + const double d = std::abs(longitudinal); + const double v = std::abs(velocity); + + if (j < 1.0e-8 || v < 1.0e-8) { + const std::string error_message( + std::string(__func__) + ": Failed to calculate lateral distance due to invalid arg"); + RCLCPP_WARN(get_logger(), "%s", error_message.c_str()); + return 1.0e10; + } + return 2.0 * std::pow(d / (4.0 * v), 3.0) * j; +} + +double calc_longitudinal_dist_from_jerk( + const double lateral, const double jerk, const double velocity) +{ + const double j = std::abs(jerk); + const double l = std::abs(lateral); + const double v = std::abs(velocity); + if (j < 1.0e-8) { + const std::string error_message( + std::string(__func__) + ": Failed to calculate longitudinal distance due to invalid arg"); + RCLCPP_WARN(get_logger(), "%s", error_message.c_str()); + return 1.0e10; + } + return 4.0 * std::pow(0.5 * l / j, 1.0 / 3.0) * v; +} + +double calc_shift_time_from_jerk(const double lateral, const double jerk, const double acc) +{ + const double j = std::abs(jerk); + const double a = std::abs(acc); + const double l = std::abs(lateral); + if (j < 1.0e-8 || a < 1.0e-8) { + const std::string error_message( + std::string(__func__) + ": Failed to calculate shift time due to invalid arg"); + RCLCPP_WARN(get_logger(), "%s", error_message.c_str()); + return 1.0e10; // TODO(Horibe) maybe invalid arg? + } + + // time with constant jerk + double tj = a / j; + + // time with constant acceleration (zero jerk) + double ta = (std::sqrt(a * a + 4.0 * j * j * l / a) - 3.0 * a) / (2.0 * j); + + if (ta < 0.0) { + // it will not hit the acceleration limit this time + tj = std::pow(l / (2.0 * j), 1.0 / 3.0); + ta = 0.0; + } + + const double t_total = 4.0 * tj + 2.0 * ta; + return t_total; +} + +double calc_jerk_from_lat_lon_distance( + const double lateral, const double longitudinal, const double velocity) +{ + constexpr double ep = 1.0e-3; + const double lat = std::abs(lateral); + const double lon = std::max(std::abs(longitudinal), ep); + const double v = std::abs(velocity); + return 0.5 * lat * std::pow(4.0 * v / lon, 3); +} + +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp b/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp new file mode 100644 index 00000000..4f9c3299 --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory/path_with_lane_id.cpp @@ -0,0 +1,139 @@ +// 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/motion_utils/trajectory/path_with_lane_id.hpp" + +#include "autoware/interpolation/spline_interpolation_points_2d.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" + +#include +#include +#include + +namespace autoware::motion_utils +{ + +std::optional> getPathIndexRangeWithLaneId( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) +{ + size_t start_idx = 0; // NOTE: to prevent from maybe-uninitialized error + size_t end_idx = 0; // NOTE: to prevent from maybe-uninitialized error + + bool found_first_idx = false; + for (size_t i = 0; i < path.points.size(); ++i) { + const auto & p = path.points.at(i); + for (const auto & id : p.lane_ids) { + if (id == target_lane_id) { + if (!found_first_idx) { + start_idx = i; + found_first_idx = true; + } + end_idx = i; + } + } + } + + if (found_first_idx) { + // NOTE: In order to fully cover lanes with target_lane_id, start_idx and end_idx are expanded. + start_idx = static_cast(std::max(0, static_cast(start_idx) - 1)); + end_idx = std::min(path.points.size() - 1, end_idx + 1); + + return std::make_pair(start_idx, end_idx); + } + + return {}; +} + +size_t findNearestIndexFromLaneId( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & pos, const int64_t lane_id) +{ + const auto opt_range = getPathIndexRangeWithLaneId(path, lane_id); + if (opt_range) { + const size_t start_idx = opt_range->first; + const size_t end_idx = opt_range->second; + + validateNonEmpty(path.points); + + const auto sub_points = std::vector{ + path.points.begin() + start_idx, path.points.begin() + end_idx + 1}; + validateNonEmpty(sub_points); + + return start_idx + findNearestIndex(sub_points, pos); + } + + return findNearestIndex(path.points, pos); +} + +size_t findNearestSegmentIndexFromLaneId( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & pos, const int64_t lane_id) +{ + const size_t nearest_idx = findNearestIndexFromLaneId(path, pos, lane_id); + + if (nearest_idx == 0) { + return 0; + } + if (nearest_idx == path.points.size() - 1) { + return path.points.size() - 2; + } + + const double signed_length = calcLongitudinalOffsetToSegment(path.points, nearest_idx, pos); + + if (signed_length <= 0) { + return nearest_idx - 1; + } + + return nearest_idx; +} + +// NOTE: rear_to_cog is supposed to be positive +autoware_internal_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( + const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, + const bool enable_last_point_compensation) +{ + auto cog_path = path; + + // calculate curvature and yaw from spline interpolation + const auto spline = autoware::interpolation::SplineInterpolationPoints2d(path.points); + const auto curvature_vec = spline.getSplineInterpolatedCurvatures(); + const auto yaw_vec = spline.getSplineInterpolatedYaws(); + + for (size_t i = 0; i < path.points.size(); ++i) { + // calculate beta, which is CoG's velocity direction + const double beta = std::atan(rear_to_cog * curvature_vec.at(i)); + + // apply beta to CoG pose + geometry_msgs::msg::Pose cog_pose_with_beta; + cog_pose_with_beta.position = autoware_utils::get_point(path.points.at(i)); + cog_pose_with_beta.orientation = + autoware_utils::create_quaternion_from_yaw(yaw_vec.at(i) - beta); + + const auto rear_pose = + autoware_utils::calc_offset_pose(cog_pose_with_beta, -rear_to_cog, 0.0, 0.0); + + // update pose + autoware_utils::set_pose(rear_pose, cog_path.points.at(i)); + } + + // compensate for the last pose + if (enable_last_point_compensation) { + cog_path.points.back() = path.points.back(); + } + + insertOrientation(cog_path.points, true); + + return cog_path; +} +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/trajectory/trajectory.cpp b/common/autoware_motion_utils/src/trajectory/trajectory.cpp new file mode 100644 index 00000000..f6d5c9f3 --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory/trajectory.cpp @@ -0,0 +1,645 @@ +// 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/motion_utils/trajectory/trajectory.hpp" + +#include +#include +#include + +namespace autoware::motion_utils +{ + +// +template void validateNonEmpty>( + const std::vector &); +template void +validateNonEmpty>( + const std::vector &); +template void validateNonEmpty>( + const std::vector &); + +// +template std::optional isDrivingForward>( + const std::vector &); +template std::optional +isDrivingForward>( + const std::vector &); +template std::optional +isDrivingForward>( + const std::vector &); + +// +template std::optional +isDrivingForwardWithTwist>( + const std::vector &); +template std::optional +isDrivingForwardWithTwist>( + const std::vector &); +template std::optional +isDrivingForwardWithTwist>( + const std::vector &); + +// +template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx); +template std::vector +removeOverlapPoints>( + const std::vector & points, + const size_t start_idx); +template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx); + +// +template std::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist, + const size_t src_idx, const size_t dst_idx); + +// +template std::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist, + const size_t src_idx); + +// +template std::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist); + +// +template size_t findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +template size_t +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +template size_t findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); + +// +template std::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); +template std::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); +template std::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); + +// +template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); +template double calcLongitudinalOffsetToSegment< + std::vector>( + const std::vector & points, + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); +template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); + +// +template size_t findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +template size_t +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +template size_t findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); + +// +template std::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); +template std::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); +template std::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); + +// +template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); +template double +calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); +template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); + +// +template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); +template double +calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); +template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); + +// +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); + +// +template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +template std::vector calcSignedArcLengthPartialSum< + std::vector>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); +template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); + +// +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); +template double +calcSignedArcLength>( + const std::vector &, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector &, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); + +// +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); +template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); + +// +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); +template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); + +// +template double calcArcLength>( + const std::vector & points); +template double +calcArcLength>( + const std::vector & points); +template double calcArcLength>( + const std::vector & points); + +// +template std::vector calc_curvature>( + const std::vector & points); +template std::vector +calc_curvature>( + const std::vector & points); +template std::vector +calc_curvature>( + const std::vector & points); + +// +template std::vector>> +calc_curvatureAndSegmentLength>( + const std::vector & points); +template std::vector>> calc_curvatureAndSegmentLength< + std::vector>( + const std::vector & points); +template std::vector>> +calc_curvatureAndSegmentLength>( + const std::vector & points); + +// +template std::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const size_t src_idx); + +// +template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception); +template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const size_t src_idx, const double offset, const bool throw_exception); +template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception); + +// +template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); +template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); +template std::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); + +// +template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction, + const bool throw_exception); +template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, + const bool throw_exception); +template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction, + const bool throw_exception); + +// +template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction); +template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction); +template std::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction); + +// +template std::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, const double overlap_threshold); +template std::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold); +template std::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold); + +// +template std::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, const double overlap_threshold); +template std::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold); +template std::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold); + +// +template std::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, const double overlap_threshold); +template std::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold); +template std::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold); + +// +template std::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, const double max_dist, + const double max_yaw, const double overlap_threshold); +template std::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, + const double max_dist, const double max_yaw, const double overlap_threshold); +template std::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, const double max_dist, + const double max_yaw, const double overlap_threshold); + +// +template std::optional insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +template std::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +template std::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); + +// +template std::optional insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold); +template std::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold); +template std::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold); + +// +template std::optional insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, const double max_dist, + const double max_yaw, const double overlap_threshold); +template std::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist, const double max_yaw, const double overlap_threshold); +template std::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist, const double max_yaw, const double overlap_threshold); + +// +template std::optional +insertStopPoint>( + const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, + std::vector & points_with_twist, + const double overlap_threshold); + +// +template std::optional +insertDecelPoint>( + const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, + const double velocity, + std::vector & points_with_twist); + +// +template void insertOrientation>( + std::vector & points, const bool is_driving_forward); +template void +insertOrientation>( + std::vector & points, + const bool is_driving_forward); +template void insertOrientation>( + std::vector & points, + const bool is_driving_forward); + +// +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); + +// +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); +template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); + +// +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); + +// +template size_t +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); +template size_t findFirstNearestIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); +template size_t +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); + +// +template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); +template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); +template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); + +// +template std::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); + +// +template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); +template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); +template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); + +// +template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); +template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); +template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); + +// +template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); +template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); +template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); + +// +template double calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception); +template double +calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception); +template double calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception); + +// +template bool isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold); +template bool +isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold); +template bool isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold); + +void calculate_time_from_start( + std::vector & trajectory, + const geometry_msgs::msg::Point & current_ego_point, const float min_velocity) +{ + const auto nearest_segment_idx = findNearestSegmentIndex(trajectory, current_ego_point); + if (nearest_segment_idx + 1 == trajectory.size()) { + return; + } + for (auto & p : trajectory) { + p.time_from_start = rclcpp::Duration::from_seconds(0); + } + // TODO(Maxime): some points can have very low velocities which introduce huge time errors + // Temporary solution: use a minimum velocity + for (auto idx = nearest_segment_idx + 1; idx < trajectory.size(); ++idx) { + const auto & from = trajectory[idx - 1]; + const auto velocity = std::max(min_velocity, from.longitudinal_velocity_mps); + if (velocity != 0.0) { + auto & to = trajectory[idx]; + const auto t = autoware_utils::calc_distance2d(from, to) / velocity; + to.time_from_start = rclcpp::Duration::from_seconds(t) + from.time_from_start; + } + } +} +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/vehicle/vehicle_state_checker.cpp b/common/autoware_motion_utils/src/vehicle/vehicle_state_checker.cpp new file mode 100644 index 00000000..e2a4754a --- /dev/null +++ b/common/autoware_motion_utils/src/vehicle/vehicle_state_checker.cpp @@ -0,0 +1,135 @@ +// 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/motion_utils/vehicle/vehicle_state_checker.hpp" + +#include "autoware/motion_utils/trajectory/trajectory.hpp" + +#include + +namespace autoware::motion_utils +{ +VehicleStopCheckerBase::VehicleStopCheckerBase(rclcpp::Node * node, double buffer_duration) +: clock_(node->get_clock()), logger_(node->get_logger()) +{ + buffer_duration_ = buffer_duration; +} + +void VehicleStopCheckerBase::addTwist(const TwistStamped & twist) +{ + twist_buffer_.push_front(twist); + + const auto now = clock_->now(); + while (!twist_buffer_.empty()) { + // Check oldest data time + const auto time_diff = now - twist_buffer_.back().header.stamp; + + // Finish when oldest data is newer than threshold + if (time_diff.seconds() <= buffer_duration_) { + break; + } + + // Remove old data + twist_buffer_.pop_back(); + } +} + +bool VehicleStopCheckerBase::isVehicleStopped(const double stop_duration) const +{ + if (twist_buffer_.empty()) { + return false; + } + + constexpr double squared_stop_velocity = 1e-3 * 1e-3; + const auto now = clock_->now(); + + const auto time_buffer_back = now - twist_buffer_.back().header.stamp; + if (time_buffer_back.seconds() < stop_duration) { + return false; + } + + // Get velocities within stop_duration + for (const auto & velocity : twist_buffer_) { + double x = velocity.twist.linear.x; + double y = velocity.twist.linear.y; + double z = velocity.twist.linear.z; + double v = (x * x) + (y * y) + (z * z); + if (squared_stop_velocity <= v) { + return false; + } + + const auto time_diff = now - velocity.header.stamp; + if (time_diff.seconds() >= stop_duration) { + break; + } + } + + return true; +} + +VehicleStopChecker::VehicleStopChecker(rclcpp::Node * node) +: VehicleStopCheckerBase(node, velocity_buffer_time_sec) +{ + using std::placeholders::_1; + + sub_odom_ = node->create_subscription( + "/localization/kinematic_state", rclcpp::QoS(1), + std::bind(&VehicleStopChecker::onOdom, this, _1)); +} + +void VehicleStopChecker::onOdom(const Odometry::ConstSharedPtr msg) +{ + odometry_ptr_ = msg; + + TwistStamped current_velocity; + current_velocity.header = msg->header; + current_velocity.twist = msg->twist.twist; + addTwist(current_velocity); +} + +VehicleArrivalChecker::VehicleArrivalChecker(rclcpp::Node * node) : VehicleStopChecker(node) +{ + using std::placeholders::_1; + + sub_trajectory_ = node->create_subscription( + "/planning/scenario_planning/trajectory", rclcpp::QoS(1), + std::bind(&VehicleArrivalChecker::onTrajectory, this, _1)); +} + +bool VehicleArrivalChecker::isVehicleStoppedAtStopPoint(const double stop_duration) const +{ + if (!odometry_ptr_ || !trajectory_ptr_) { + return false; + } + + if (!isVehicleStopped(stop_duration)) { + return false; + } + + const auto & p = odometry_ptr_->pose.pose.position; + const auto idx = autoware::motion_utils::searchZeroVelocityIndex(trajectory_ptr_->points); + + if (!idx) { + return false; + } + + return std::abs(autoware::motion_utils::calcSignedArcLength( + trajectory_ptr_->points, p, idx.value())) < th_arrived_distance_m; +} + +void VehicleArrivalChecker::onTrajectory(const Trajectory::ConstSharedPtr msg) +{ + trajectory_ptr_ = msg; +} +} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/test/src/distance/test_distance.cpp b/common/autoware_motion_utils/test/src/distance/test_distance.cpp new file mode 100644 index 00000000..f6d6a9cc --- /dev/null +++ b/common/autoware_motion_utils/test/src/distance/test_distance.cpp @@ -0,0 +1,116 @@ +// 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/motion_utils/distance/distance.hpp" +#include "gtest/gtest.h" +namespace +{ +using autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints; + +constexpr double epsilon = 1e-3; + +TEST(distance, calcDecelDistWithJerkAndAccConstraints) +{ + // invalid velocity + { + constexpr double current_vel = 16.7; + constexpr double target_vel = 20.0; + constexpr double current_acc = 0.0; + constexpr double acc_min = -0.5; + constexpr double jerk_acc = 1.0; + constexpr double jerk_dec = -0.5; + + const auto dist = calcDecelDistWithJerkAndAccConstraints( + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); + + EXPECT_FALSE(dist); + } + + // normal stop + { + constexpr double current_vel = 16.7; + constexpr double target_vel = 0.0; + constexpr double current_acc = 0.0; + constexpr double acc_min = -0.5; + constexpr double jerk_acc = 1.0; + constexpr double jerk_dec = -0.5; + + constexpr double expected_dist = 287.224; + const auto dist = calcDecelDistWithJerkAndAccConstraints( + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); + EXPECT_NEAR(expected_dist, *dist, epsilon); + } + + // sudden stop + { + constexpr double current_vel = 16.7; + constexpr double target_vel = 0.0; + constexpr double current_acc = 0.0; + constexpr double acc_min = -2.5; + constexpr double jerk_acc = 1.5; + constexpr double jerk_dec = -1.5; + + constexpr double expected_dist = 69.6947; + const auto dist = calcDecelDistWithJerkAndAccConstraints( + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); + EXPECT_NEAR(expected_dist, *dist, epsilon); + } + + // normal deceleration + { + constexpr double current_vel = 16.7; + constexpr double target_vel = 10.0; + constexpr double current_acc = 0.0; + constexpr double acc_min = -0.5; + constexpr double jerk_acc = 1.0; + constexpr double jerk_dec = -0.5; + + constexpr double expected_dist = 189.724; + const auto dist = calcDecelDistWithJerkAndAccConstraints( + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); + EXPECT_NEAR(expected_dist, *dist, epsilon); + } + + // sudden deceleration + { + constexpr double current_vel = 16.7; + constexpr double target_vel = 10.0; + constexpr double current_acc = 0.0; + constexpr double acc_min = -2.5; + constexpr double jerk_acc = 1.5; + constexpr double jerk_dec = -1.5; + + constexpr double expected_dist = 58.028; + const auto dist = calcDecelDistWithJerkAndAccConstraints( + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); + EXPECT_NEAR(expected_dist, *dist, epsilon); + } + + // current_acc is lower than acc_min + { + constexpr double current_vel = 16.7; + constexpr double target_vel = 0.0; + constexpr double current_acc = -2.5; + constexpr double acc_min = -0.5; + constexpr double jerk_acc = 1.0; + constexpr double jerk_dec = -0.5; + + constexpr double expected_dist = 217.429; + const auto dist = calcDecelDistWithJerkAndAccConstraints( + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec); + EXPECT_NEAR(expected_dist, *dist, epsilon); + } +} + +} // namespace diff --git a/common/autoware_motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp b/common/autoware_motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp new file mode 100644 index 00000000..afce7f95 --- /dev/null +++ b/common/autoware_motion_utils/test/src/marker/test_virtual_wall_marker_creator.cpp @@ -0,0 +1,117 @@ +// 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/motion_utils/marker/virtual_wall_marker_creator.hpp" +#include "gtest/gtest.h" + +#include + +namespace +{ +constexpr auto wall_ns_suffix = "_virtual_wall"; +constexpr auto text_ns_suffix = "_factor_text"; + +bool has_ns_id( + const visualization_msgs::msg::MarkerArray & marker_array, const std::string & ns, const int id) +{ + return std::find_if(marker_array.markers.begin(), marker_array.markers.end(), [&](const auto m) { + return m.id == id && m.ns == ns; + }) != marker_array.markers.end(); +} + +bool has_ns_id( + const visualization_msgs::msg::MarkerArray & marker_array, const std::string & ns, + const int id_from, const int id_to) +{ + for (auto id = id_from; id <= id_to; ++id) + if (!has_ns_id(marker_array, ns, id)) return false; + return true; +} + +TEST(VirtualWallMarkerCreator, oneWall) +{ + autoware::motion_utils::VirtualWall wall; + autoware::motion_utils::VirtualWallMarkerCreator creator; + wall.style = autoware::motion_utils::VirtualWallType::stop; + wall.pose.position.x = 1.0; + wall.pose.position.y = 2.0; + creator.add_virtual_wall(wall); + auto markers = creator.create_markers(); + ASSERT_EQ(markers.markers.size(), 2UL); + EXPECT_TRUE(has_ns_id(markers, std::string("stop") + wall_ns_suffix, 0)); + EXPECT_TRUE(has_ns_id(markers, std::string("stop") + text_ns_suffix, 0)); + for (const auto & marker : markers.markers) { + EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::ADD); + EXPECT_EQ(marker.pose.position.x, 1.0); + EXPECT_EQ(marker.pose.position.y, 2.0); + } + markers = creator.create_markers(); + ASSERT_EQ(markers.markers.size(), 2UL); + EXPECT_TRUE(has_ns_id(markers, std::string("stop") + wall_ns_suffix, 0)); + EXPECT_TRUE(has_ns_id(markers, std::string("stop") + text_ns_suffix, 0)); + for (const auto & marker : markers.markers) + EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::DELETE); +} + +TEST(VirtualWallMarkerCreator, manyWalls) +{ + autoware::motion_utils::VirtualWall wall; + autoware::motion_utils::VirtualWallMarkerCreator creator; + wall.style = autoware::motion_utils::VirtualWallType::stop; + wall.ns = "ns1_"; + creator.add_virtual_wall(wall); + creator.add_virtual_wall(wall); + creator.add_virtual_wall(wall); + wall.ns = "ns2_"; + creator.add_virtual_wall(wall); + wall.style = autoware::motion_utils::VirtualWallType::slowdown; + wall.ns = "ns2_"; + creator.add_virtual_wall(wall); + creator.add_virtual_wall(wall); + wall.ns = "ns3_"; + creator.add_virtual_wall(wall); + creator.add_virtual_wall(wall); + creator.add_virtual_wall(wall); + wall.style = autoware::motion_utils::VirtualWallType::deadline; + wall.ns = "ns1_"; + creator.add_virtual_wall(wall); + wall.ns = "ns2_"; + creator.add_virtual_wall(wall); + wall.ns = "ns3_"; + creator.add_virtual_wall(wall); + + auto markers = creator.create_markers(); + ASSERT_EQ(markers.markers.size(), 12UL * 2); + EXPECT_TRUE(has_ns_id(markers, std::string("ns1_stop") + wall_ns_suffix, 0, 2)); + EXPECT_TRUE(has_ns_id(markers, std::string("ns1_stop") + text_ns_suffix, 0, 2)); + EXPECT_TRUE(has_ns_id(markers, std::string("ns2_stop") + wall_ns_suffix, 0)); + EXPECT_TRUE(has_ns_id(markers, std::string("ns2_stop") + text_ns_suffix, 0)); + EXPECT_TRUE(has_ns_id(markers, std::string("ns2_slow_down") + wall_ns_suffix, 0, 1)); + EXPECT_TRUE(has_ns_id(markers, std::string("ns2_slow_down") + text_ns_suffix, 0, 1)); + EXPECT_TRUE(has_ns_id(markers, std::string("ns3_slow_down") + wall_ns_suffix, 0, 2)); + EXPECT_TRUE(has_ns_id(markers, std::string("ns3_slow_down") + text_ns_suffix, 0, 2)); + EXPECT_TRUE(has_ns_id(markers, std::string("ns1_dead_line") + wall_ns_suffix, 0)); + EXPECT_TRUE(has_ns_id(markers, std::string("ns1_dead_line") + text_ns_suffix, 0)); + EXPECT_TRUE(has_ns_id(markers, std::string("ns2_dead_line") + wall_ns_suffix, 0)); + EXPECT_TRUE(has_ns_id(markers, std::string("ns2_dead_line") + text_ns_suffix, 0)); + EXPECT_TRUE(has_ns_id(markers, std::string("ns3_dead_line") + wall_ns_suffix, 0)); + EXPECT_TRUE(has_ns_id(markers, std::string("ns3_dead_line") + text_ns_suffix, 0)); + markers = creator.create_markers(); + ASSERT_EQ(markers.markers.size(), 12UL * 2); + for (const auto & marker : markers.markers) + EXPECT_EQ(marker.action, visualization_msgs::msg::Marker::DELETE); + markers = creator.create_markers(); + ASSERT_TRUE(markers.markers.empty()); +} +} // namespace diff --git a/common/autoware_motion_utils/test/src/resample/test_resample.cpp b/common/autoware_motion_utils/test/src/resample/test_resample.cpp new file mode 100644 index 00000000..a7ff8037 --- /dev/null +++ b/common/autoware_motion_utils/test/src/resample/test_resample.cpp @@ -0,0 +1,3622 @@ +// 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/motion_utils/constants.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware_utils/geometry/boost_geometry.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/constants.hpp" +#include "autoware_utils/math/unit_conversion.hpp" + +#include +#include +#include + +#include +#include + +namespace +{ +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_utils::create_point; +using autoware_utils::create_quaternion_from_rpy; +using autoware_utils::transform_point; + +constexpr double epsilon = 1e-6; + +geometry_msgs::msg::Pose createPose( + double x, double y, double z, double roll, double pitch, double yaw) +{ + geometry_msgs::msg::Pose p; + p.position = create_point(x, y, z); + p.orientation = create_quaternion_from_rpy(roll, pitch, yaw); + return p; +} + +PathPointWithLaneId generateTestPathPointWithLaneId( + const double x, const double y, const double z, const double theta = 0.0, + const double vel_lon = 0.0, const double vel_lat = 0.0, const double heading_rate = 0.0, + const bool is_final = false, const std::vector & lane_ids = {}) +{ + PathPointWithLaneId p; + p.point.pose = createPose(x, y, z, 0.0, 0.0, theta); + p.point.longitudinal_velocity_mps = vel_lon; + p.point.lateral_velocity_mps = vel_lat; + p.point.heading_rate_rps = heading_rate; + p.point.is_final = is_final; + p.lane_ids = lane_ids; + return p; +} + +PathPoint generateTestPathPoint( + const double x, const double y, const double z, const double theta = 0.0, + const double vel_lon = 0.0, const double vel_lat = 0.0, const double heading_rate = 0.0) +{ + PathPoint p; + p.pose = createPose(x, y, z, 0.0, 0.0, theta); + p.longitudinal_velocity_mps = vel_lon; + p.lateral_velocity_mps = vel_lat; + p.heading_rate_rps = heading_rate; + return p; +} + +TrajectoryPoint generateTestTrajectoryPoint( + const double x, const double y, const double z, const double theta = 0.0, + const double vel_lon = 0.0, const double vel_lat = 0.0, const double heading_rate = 0.0, + const double acc = 0.0) +{ + TrajectoryPoint p; + p.pose = createPose(x, y, z, 0.0, 0.0, theta); + p.longitudinal_velocity_mps = vel_lon; + p.lateral_velocity_mps = vel_lat; + p.heading_rate_rps = heading_rate; + p.acceleration_mps2 = acc; + return p; +} + +PathWithLaneId generateTestPathWithLaneId( + const size_t num_points, const double point_interval, const double vel_lon = 0.0, + const double vel_lat = 0.0, const double heading_rate_rps = 0.0, const double init_theta = 0.0, + const double delta_theta = 0.0) +{ + PathWithLaneId path; + for (size_t i = 0; i < num_points; ++i) { + const double theta = init_theta + i * delta_theta; + const double x = i * point_interval * std::cos(theta); + const double y = i * point_interval * std::sin(theta); + + PathPointWithLaneId p; + p.point.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); + p.point.longitudinal_velocity_mps = vel_lon; + p.point.lateral_velocity_mps = vel_lat; + p.point.heading_rate_rps = heading_rate_rps; + p.point.is_final = (i == num_points - 1); + p.lane_ids = {static_cast(i)}; + path.points.push_back(p); + } + + return path; +} + +template +T generateTestPath( + const size_t num_points, const double point_interval, const double vel_lon = 0.0, + const double vel_lat = 0.0, const double heading_rate_rps = 0.0, const double init_theta = 0.0, + const double delta_theta = 0.0) +{ + using Point = typename T::_points_type::value_type; + + T traj; + for (size_t i = 0; i < num_points; ++i) { + const double theta = init_theta + i * delta_theta; + const double x = i * point_interval * std::cos(theta); + const double y = i * point_interval * std::sin(theta); + + Point p; + p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); + p.longitudinal_velocity_mps = vel_lon; + p.lateral_velocity_mps = vel_lat; + p.heading_rate_rps = heading_rate_rps; + traj.points.push_back(p); + } + + return traj; +} + +template +T generateTestTrajectory( + const size_t num_points, const double point_interval, const double vel_lon = 0.0, + const double vel_lat = 0.0, const double heading_rate_rps = 0.0, const double acc = 0.0, + const double init_theta = 0.0, const double delta_theta = 0.0) +{ + using Point = typename T::_points_type::value_type; + + T traj; + for (size_t i = 0; i < num_points; ++i) { + const double theta = init_theta + i * delta_theta; + const double x = i * point_interval * std::cos(theta); + const double y = i * point_interval * std::sin(theta); + + Point p; + p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); + p.longitudinal_velocity_mps = vel_lon; + p.lateral_velocity_mps = vel_lat; + p.heading_rate_rps = heading_rate_rps; + p.acceleration_mps2 = acc; + traj.points.push_back(p); + } + + return traj; +} + +std::vector generateArclength(const size_t num_points, const double interval) +{ + std::vector resampled_arclength(num_points); + for (size_t i = 0; i < num_points; ++i) { + resampled_arclength.at(i) = i * interval; + } + + return resampled_arclength; +} + +template +std::vector setZeroVelocityAfterStop(const std::vector & traj_points) +{ + std::vector resampled_traj_points; + bool stop_point_found = false; + for (auto p : traj_points) { + if (!stop_point_found && p.longitudinal_velocity_mps < std::numeric_limits::epsilon()) { + stop_point_found = true; + } + if (stop_point_found) { + p.longitudinal_velocity_mps = 0.0; + } + resampled_traj_points.push_back(p); + } + return resampled_traj_points; +} + +} // namespace + +TEST(resample_vector_pose, resample_by_same_interval) +{ + using autoware::motion_utils::resamplePoseVector; + using geometry_msgs::msg::Pose; + + std::vector path(10); + for (size_t i = 0; i < 10; ++i) { + path.at(i) = createPose(i * 1.0, 0.0, 0.0, 0.0, 0.0, 0.0); + } + + // same interval + { + const auto resampled_path = resamplePoseVector(path, 1.0); + EXPECT_EQ(path.size(), resampled_path.size()); + for (size_t i = 0; i < path.size(); ++i) { + const auto & p = resampled_path.at(i); + const auto & ans_p = path.at(i); + EXPECT_NEAR(p.position.x, ans_p.position.x, epsilon); + EXPECT_NEAR(p.position.y, ans_p.position.y, epsilon); + EXPECT_NEAR(p.position.z, ans_p.position.z, epsilon); + EXPECT_NEAR(p.orientation.x, ans_p.orientation.x, epsilon); + EXPECT_NEAR(p.orientation.y, ans_p.orientation.y, epsilon); + EXPECT_NEAR(p.orientation.z, ans_p.orientation.z, epsilon); + EXPECT_NEAR(p.orientation.w, ans_p.orientation.w, epsilon); + } + } + + // random + { + const auto resampled_path = resamplePoseVector(path, 0.5); + for (size_t i = 0; i < path.size(); ++i) { + const auto & p = resampled_path.at(i); + EXPECT_NEAR(p.position.x, 0.5 * i, epsilon); + EXPECT_NEAR(p.position.y, 0.0, epsilon); + EXPECT_NEAR(p.position.z, 0.0, epsilon); + EXPECT_NEAR(p.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.orientation.w, 1.0, epsilon); + } + } +} + +TEST(resample_path_with_lane_id, resample_path_by_vector) +{ + using autoware::motion_utils::resamplePath; + // Output is same as input + { + auto path = generateTestPathWithLaneId(10, 1.0, 3.0, 1.0, 0.01); + std::vector resampled_arclength = generateArclength(10, 1.0); + + { + const auto resampled_path = resamplePath(path, resampled_arclength); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + const auto ans_p = path.points.at(i); + EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon); + EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon); + EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon); + EXPECT_NEAR( + p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); + EXPECT_EQ(p.point.is_final, ans_p.point.is_final); + for (size_t j = 0; j < p.lane_ids.size(); ++j) { + EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j)); + } + } + } + + // Change the last point orientation + path.points.back() = generateTestPathPointWithLaneId( + 9.0, 0.0, 0.0, autoware_utils::pi / 3.0, 3.0, 1.0, 0.01, true, {9}); + { + const auto resampled_path = resamplePath(path, resampled_arclength); + for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { + const auto p = resampled_path.points.at(i); + const auto ans_p = path.points.at(i); + EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon); + EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon); + EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon); + EXPECT_NEAR( + p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); + EXPECT_EQ(p.point.is_final, ans_p.point.is_final); + for (size_t j = 0; j < p.lane_ids.size(); ++j) { + EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j)); + } + } + + const auto p = resampled_path.points.back(); + const auto ans_p = path.points.back(); + const auto ans_quat = autoware_utils::create_quaternion(0.0, 0.0, 0.0, 1.0); + EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon); + EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon); + EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, ans_quat.w, epsilon); + EXPECT_NEAR( + p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); + EXPECT_EQ(p.point.is_final, ans_p.point.is_final); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), ans_p.lane_ids.at(i)); + } + } + } + + // Output key is not same as input + { + autoware_internal_planning_msgs::msg::PathWithLaneId path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPointWithLaneId( + i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); + } + path.points.back().point.is_final = true; + std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; + + const auto resampled_path = resamplePath(path, resampled_arclength); + + { + const auto p = resampled_path.points.at(0); + EXPECT_NEAR(p.point.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.0, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 0); + } + } + + { + const auto p = resampled_path.points.at(1); + EXPECT_NEAR(p.point.pose.position.x, 1.2, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.12, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 1); + } + } + + { + const auto p = resampled_path.points.at(2); + EXPECT_NEAR(p.point.pose.position.x, 1.5, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.15, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 1); + } + } + + { + const auto p = resampled_path.points.at(3); + EXPECT_NEAR(p.point.pose.position.x, 5.3, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 5.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 2.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.53, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 5); + } + } + + { + const auto p = resampled_path.points.at(4); + EXPECT_NEAR(p.point.pose.position.x, 7.5, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 7.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 3.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.75, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 7); + } + } + + { + const auto p = resampled_path.points.at(5); + EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon); + EXPECT_EQ(p.point.is_final, true); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 9); + } + } + + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon); + } + } + + // Duplicated points in the original path + { + autoware_internal_planning_msgs::msg::PathWithLaneId path; + path.points.resize(11); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPointWithLaneId( + i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); + } + path.points.at(6) = path.points.at(5); + std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; + + const auto resampled_path = resamplePath(path, resampled_arclength); + + EXPECT_EQ(path.points.size(), resampled_path.points.size()); + for (size_t i = 0; i < path.points.size(); ++i) { + const auto p = path.points.at(i); + const auto resampled_p = resampled_path.points.at(i); + EXPECT_NEAR(p.point.pose.position.x, resampled_p.point.pose.position.x, epsilon); + EXPECT_NEAR(p.point.pose.position.y, resampled_p.point.pose.position.y, epsilon); + EXPECT_NEAR(p.point.pose.position.z, resampled_p.point.pose.position.z, epsilon); + EXPECT_NEAR( + p.point.longitudinal_velocity_mps, resampled_p.point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, resampled_p.point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, resampled_p.point.heading_rate_rps, epsilon); + EXPECT_EQ(p.point.is_final, resampled_p.point.is_final); + EXPECT_NEAR(p.point.pose.orientation.x, resampled_p.point.pose.orientation.x, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, resampled_p.point.pose.orientation.y, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, resampled_p.point.pose.orientation.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, resampled_p.point.pose.orientation.w, epsilon); + } + } + + // No Interpolation + { + // Input path size is not enough for interpolation + { + autoware_internal_planning_msgs::msg::PathWithLaneId path; + path.points.resize(1); + for (size_t i = 0; i < 1; ++i) { + path.points.at(i) = generateTestPathPointWithLaneId( + i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, true, {0}); + } + std::vector resampled_arclength = generateArclength(10, 1.0); + + const auto resampled_path = resamplePath(path, resampled_arclength); + EXPECT_EQ(resampled_path.points.size(), path.points.size()); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + const auto ans_p = path.points.at(i); + EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon); + EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon); + EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon); + EXPECT_NEAR( + p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); + EXPECT_EQ(p.point.is_final, ans_p.point.is_final); + for (size_t j = 0; j < p.lane_ids.size(); ++j) { + EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j)); + } + } + } + + // Resampled Arclength size is not enough for interpolation + { + autoware_internal_planning_msgs::msg::PathWithLaneId path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPointWithLaneId( + i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, true, {static_cast(i)}); + } + path.points.back().point.is_final = false; + std::vector resampled_arclength = generateArclength(1, 1.0); + + const auto resampled_path = resamplePath(path, resampled_arclength); + EXPECT_EQ(resampled_path.points.size(), path.points.size()); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + const auto ans_p = path.points.at(i); + EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon); + EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon); + EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon); + EXPECT_NEAR( + p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); + EXPECT_EQ(p.point.is_final, ans_p.point.is_final); + for (size_t j = 0; j < p.lane_ids.size(); ++j) { + EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j)); + } + } + } + + // Resampled Arclength is longer than input path + { + autoware_internal_planning_msgs::msg::PathWithLaneId path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPointWithLaneId( + i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, true, {static_cast(i)}); + } + path.points.back().point.is_final = false; + std::vector resampled_arclength = generateArclength(3, 5.0); + + const auto resampled_path = resamplePath(path, resampled_arclength); + EXPECT_EQ(resampled_path.points.size(), path.points.size()); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + const auto ans_p = path.points.at(i); + EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon); + EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon); + EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon); + EXPECT_NEAR( + p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); + EXPECT_EQ(p.point.is_final, ans_p.point.is_final); + for (size_t j = 0; j < p.lane_ids.size(); ++j) { + EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j)); + } + } + } + } +} + +TEST(resample_path_with_lane_id, resample_path_by_vector_backward) +{ + using autoware::motion_utils::resamplePath; + + { + autoware_internal_planning_msgs::msg::PathWithLaneId path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPointWithLaneId( + i * 1.0, 0.0, 0.0, autoware_utils::pi, i * 1.0, i * 0.5, i * 0.1, false, + {static_cast(i)}); + } + path.points.back().point.is_final = true; + std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; + + const auto resampled_path = resamplePath(path, resampled_arclength); + + { + const auto p = resampled_path.points.at(0); + EXPECT_NEAR(p.point.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.0, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 0); + } + } + + { + const auto p = resampled_path.points.at(1); + EXPECT_NEAR(p.point.pose.position.x, 1.2, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.12, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 1); + } + } + + { + const auto p = resampled_path.points.at(2); + EXPECT_NEAR(p.point.pose.position.x, 1.5, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.15, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 1); + } + } + + { + const auto p = resampled_path.points.at(3); + EXPECT_NEAR(p.point.pose.position.x, 5.3, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 5.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 2.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.53, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 5); + } + } + + { + const auto p = resampled_path.points.at(4); + EXPECT_NEAR(p.point.pose.position.x, 7.5, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 7.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 3.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.75, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 7); + } + } + + { + const auto p = resampled_path.points.at(5); + EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon); + EXPECT_EQ(p.point.is_final, true); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 9); + } + } + + const auto ans_quat = autoware_utils::create_quaternion_from_yaw(M_PI); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i).point; + EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); + } + } + + // change initial orientation + { + autoware_internal_planning_msgs::msg::PathWithLaneId path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPointWithLaneId( + i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); + } + path.points.back().point.is_final = true; + path.points.at(0).point.pose.orientation = + autoware_utils::create_quaternion_from_yaw(M_PI + M_PI / 3.0); + std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; + + const auto resampled_path = resamplePath(path, resampled_arclength); + + { + const auto p = resampled_path.points.at(0); + EXPECT_NEAR(p.point.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.0, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 0); + } + } + + { + const auto p = resampled_path.points.at(1); + EXPECT_NEAR(p.point.pose.position.x, 1.2, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.12, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 1); + } + } + + { + const auto p = resampled_path.points.at(2); + EXPECT_NEAR(p.point.pose.position.x, 1.5, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.15, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 1); + } + } + + { + const auto p = resampled_path.points.at(3); + EXPECT_NEAR(p.point.pose.position.x, 5.3, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 5.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 2.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.53, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 5); + } + } + + { + const auto p = resampled_path.points.at(4); + EXPECT_NEAR(p.point.pose.position.x, 7.5, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 7.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 3.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.75, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 7); + } + } + + { + const auto p = resampled_path.points.at(5); + EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon); + EXPECT_EQ(p.point.is_final, true); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 9); + } + } + + // Initial Orientation + { + const auto ans_quat = autoware_utils::create_quaternion_from_yaw(M_PI + M_PI / 3.0); + const auto p = resampled_path.points.at(0).point; + EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); + } + + const auto ans_quat = autoware_utils::create_quaternion_from_yaw(M_PI); + for (size_t i = 1; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i).point; + EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); + } + } +} + +TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) +{ + using autoware::motion_utils::resamplePath; + + // Lerp x, y + { + autoware_internal_planning_msgs::msg::PathWithLaneId path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPointWithLaneId( + i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); + } + path.points.back().point.is_final = true; + std::vector resampled_arclength = {0.0, 1.2, 5.3, 9.0}; + + const auto resampled_path = resamplePath(path, resampled_arclength, true); + { + const auto p = resampled_path.points.at(0); + EXPECT_NEAR(p.point.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.0, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 0); + } + } + + { + const auto p = resampled_path.points.at(1); + EXPECT_NEAR(p.point.pose.position.x, 1.2, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.12, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 1); + } + } + + { + const auto p = resampled_path.points.at(2); + EXPECT_NEAR(p.point.pose.position.x, 5.3, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 5.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 2.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.53, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 5); + } + } + + { + const auto p = resampled_path.points.at(3); + EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon); + EXPECT_EQ(p.point.is_final, true); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 9); + } + } + + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i).point; + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + } + } + + // Slerp z + { + autoware_internal_planning_msgs::msg::PathWithLaneId path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPointWithLaneId( + i * 1.0, 0.0, i * 1.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); + } + path.points.back().point.is_final = true; + std::vector resampled_arclength = {0.0, 1.2, 5.3, 9.0}; + + const auto resampled_path = resamplePath(path, resampled_arclength, false, false); + { + const auto p = resampled_path.points.at(0); + EXPECT_NEAR(p.point.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.0, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 0); + } + } + + { + const auto p = resampled_path.points.at(1); + EXPECT_NEAR(p.point.pose.position.x, 1.2, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 1.2, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.12, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 1); + } + } + + { + const auto p = resampled_path.points.at(2); + EXPECT_NEAR(p.point.pose.position.x, 5.3, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 5.3, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 5.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 2.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.53, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 5); + } + } + + { + const auto p = resampled_path.points.at(3); + EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 9.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon); + EXPECT_EQ(p.point.is_final, true); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 9); + } + } + + const double pitch = std::atan(1.0); + const auto ans_quat = autoware_utils::create_quaternion_from_rpy(0.0, pitch, 0.0); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i).point; + EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); + } + } + + // Lerp v + { + autoware_internal_planning_msgs::msg::PathWithLaneId path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPointWithLaneId( + i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); + } + path.points.back().point.is_final = true; + std::vector resampled_arclength = {0.0, 1.2, 5.3, 9.0}; + + const auto resampled_path = resamplePath(path, resampled_arclength, false, true, false); + { + const auto p = resampled_path.points.at(0); + EXPECT_NEAR(p.point.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.0, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 0); + } + } + + { + const auto p = resampled_path.points.at(1); + EXPECT_NEAR(p.point.pose.position.x, 1.2, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.2, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 0.6, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.12, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 1); + } + } + + { + const auto p = resampled_path.points.at(2); + EXPECT_NEAR(p.point.pose.position.x, 5.3, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 5.3, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 2.65, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.53, epsilon); + EXPECT_EQ(p.point.is_final, false); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 5); + } + } + + { + const auto p = resampled_path.points.at(3); + EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon); + EXPECT_EQ(p.point.is_final, true); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), 9); + } + } + + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i).point; + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + } + } +} + +TEST(resample_path_with_lane_id, resample_path_by_same_interval) +{ + using autoware::motion_utils::resamplePath; + + // Same point resampling + { + PathWithLaneId path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPointWithLaneId( + i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); + } + path.points.back().point.is_final = true; + + { + const auto resampled_path = resamplePath(path, 1.0); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + const auto ans_p = path.points.at(i); + EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon); + EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon); + EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon); + EXPECT_NEAR( + p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); + EXPECT_EQ(p.point.is_final, ans_p.point.is_final); + for (size_t j = 0; j < p.lane_ids.size(); ++j) { + EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j)); + } + } + } + // Change the last point orientation + path.points.back() = generateTestPathPointWithLaneId( + 9.0, 0.0, 0.0, autoware_utils::pi / 3.0, 3.0, 1.0, 0.01, true, {9}); + { + const auto resampled_path = resamplePath(path, 1.0); + for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { + const auto p = resampled_path.points.at(i); + const auto ans_p = path.points.at(i); + EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon); + EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon); + EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon); + EXPECT_NEAR( + p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); + EXPECT_EQ(p.point.is_final, ans_p.point.is_final); + for (size_t j = 0; j < p.lane_ids.size(); ++j) { + EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j)); + } + } + + const auto p = resampled_path.points.back(); + const auto ans_p = path.points.back(); + const auto ans_quat = autoware_utils::create_quaternion_from_yaw(0.0); + EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon); + EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon); + EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, ans_quat.w, epsilon); + EXPECT_NEAR( + p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); + EXPECT_EQ(p.point.is_final, ans_p.point.is_final); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), ans_p.lane_ids.at(i)); + } + } + } + + // Normal Case without zero point + { + PathWithLaneId path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPointWithLaneId( + i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); + } + path.points.at(0).point.longitudinal_velocity_mps = 5.0; + + const auto resampled_path = resamplePath(path, 0.1); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + EXPECT_NEAR(p.point.pose.position.x, 0.1 * i, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon); + + const size_t idx = i / 10; + EXPECT_NEAR( + p.point.longitudinal_velocity_mps, path.points.at(idx).point.longitudinal_velocity_mps, + epsilon); + EXPECT_NEAR( + p.point.lateral_velocity_mps, path.points.at(idx).point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.01 * i, epsilon); + EXPECT_EQ(p.point.is_final, path.points.at(idx).point.is_final); + for (size_t j = 0; j < p.lane_ids.size(); ++j) { + EXPECT_EQ(p.lane_ids.at(j), path.points.at(idx).lane_ids.at(j)); + } + } + } + + // Normal Case without stop point but with terminal point + { + PathWithLaneId path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPointWithLaneId( + i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); + } + path.points.at(0).point.longitudinal_velocity_mps = 5.0; + path.points.back().point.is_final = true; + + const auto resampled_traj = resamplePath(path, 0.4); + for (size_t i = 0; i < resampled_traj.points.size() - 1; ++i) { + const auto p = resampled_traj.points.at(i); + EXPECT_NEAR(p.point.pose.position.x, 0.4 * i, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon); + + const size_t idx = i / 2.5; + EXPECT_NEAR( + p.point.longitudinal_velocity_mps, path.points.at(idx).point.longitudinal_velocity_mps, + epsilon); + EXPECT_NEAR( + p.point.lateral_velocity_mps, path.points.at(idx).point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.04 * i, epsilon); + EXPECT_EQ(p.point.is_final, path.points.at(idx).point.is_final); + for (size_t j = 0; j < p.lane_ids.size(); ++j) { + EXPECT_EQ(p.lane_ids.at(j), path.points.at(idx).lane_ids.at(j)); + } + } + + { + const auto p = resampled_traj.points.at(23); + EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon); + + const size_t idx = 9; + EXPECT_NEAR( + p.point.longitudinal_velocity_mps, path.points.at(idx).point.longitudinal_velocity_mps, + epsilon); + EXPECT_NEAR( + p.point.lateral_velocity_mps, path.points.at(idx).point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon); + EXPECT_EQ(p.point.is_final, path.points.at(idx).point.is_final); + for (size_t i = 0; i < p.lane_ids.size(); ++i) { + EXPECT_EQ(p.lane_ids.at(i), path.points.at(idx).lane_ids.at(i)); + } + } + } + + // Normal Case without stop point but with terminal point (Boundary Condition) + { + PathWithLaneId path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPointWithLaneId( + i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); + } + path.points.at(0).point.longitudinal_velocity_mps = 5.0; + path.points.back().point.is_final = true; + + const double ds = 1.0 - autoware::motion_utils::overlap_threshold; + const auto resampled_path = resamplePath(path, ds); + for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { + const auto p = resampled_path.points.at(i); + EXPECT_NEAR(p.point.pose.position.x, ds * i, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon); + + const size_t idx = i == 0 ? 0 : i - 1; + EXPECT_NEAR( + p.point.longitudinal_velocity_mps, path.points.at(idx).point.longitudinal_velocity_mps, + epsilon); + EXPECT_NEAR( + p.point.lateral_velocity_mps, path.points.at(idx).point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, ds / 10 * i, epsilon); + EXPECT_EQ(p.point.is_final, path.points.at(idx).point.is_final); + for (size_t j = 0; j < p.lane_ids.size(); ++j) { + EXPECT_EQ(p.lane_ids.at(j), path.points.at(idx).lane_ids.at(j)); + } + } + + { + const auto p = resampled_path.points.at(10); + EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon); + + const size_t idx = 9; + EXPECT_NEAR( + p.point.longitudinal_velocity_mps, path.points.at(idx).point.longitudinal_velocity_mps, + epsilon); + EXPECT_NEAR( + p.point.lateral_velocity_mps, path.points.at(idx).point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon); + EXPECT_EQ(p.point.is_final, path.points.at(idx).point.is_final); + for (size_t j = 0; j < p.lane_ids.size(); ++j) { + EXPECT_EQ(p.lane_ids.at(j), path.points.at(idx).lane_ids.at(j)); + } + } + } + + // Normal Case with duplicated zero point + { + PathWithLaneId path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPointWithLaneId( + i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); + } + path.points.at(0).point.longitudinal_velocity_mps = 5.0; + path.points.at(5).point.longitudinal_velocity_mps = 0.0; + path.points.back().point.is_final = true; + + const auto resampled_path = resamplePath(path, 0.1); + EXPECT_EQ(resampled_path.points.size(), static_cast(91)); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + EXPECT_NEAR(p.point.pose.position.x, 0.1 * i, epsilon); + EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon); + + const size_t idx = i / 10; + EXPECT_NEAR( + p.point.longitudinal_velocity_mps, path.points.at(idx).point.longitudinal_velocity_mps, + epsilon); + EXPECT_NEAR( + p.point.lateral_velocity_mps, path.points.at(idx).point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, 0.01 * i, epsilon); + for (size_t j = 0; j < p.lane_ids.size(); ++j) { + EXPECT_EQ(p.lane_ids.at(j), path.points.at(idx).lane_ids.at(j)); + } + } + } + + // Normal Case with zero point + { + PathWithLaneId path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPointWithLaneId( + i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); + } + path.points.at(0).point.longitudinal_velocity_mps = 8.0; + path.points.at(5).point.longitudinal_velocity_mps = 0.0; + path.points.back().point.is_final = true; + + const auto resampled_path = resamplePath(path, 1.5); + EXPECT_EQ(resampled_path.points.size(), static_cast(8)); + { + const auto p = resampled_path.points.at(0).point; + const auto lane_ids = resampled_path.points.at(0).lane_ids; + EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR( + p.longitudinal_velocity_mps, path.points.at(0).point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(0).point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); + EXPECT_EQ(p.is_final, false); + for (size_t j = 0; j < lane_ids.size(); ++j) { + EXPECT_EQ(lane_ids.at(j), path.points.at(0).lane_ids.at(j)); + } + } + + { + const auto p = resampled_path.points.at(1).point; + const auto lane_ids = resampled_path.points.at(1).lane_ids; + EXPECT_NEAR(p.pose.position.x, 1.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR( + p.longitudinal_velocity_mps, path.points.at(1).point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(1).point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.15, epsilon); + EXPECT_EQ(p.is_final, false); + for (size_t j = 0; j < lane_ids.size(); ++j) { + EXPECT_EQ(lane_ids.at(j), path.points.at(1).lane_ids.at(j)); + } + } + + { + const auto p = resampled_path.points.at(2).point; + const auto lane_ids = resampled_path.points.at(2).lane_ids; + EXPECT_NEAR(p.pose.position.x, 3.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR( + p.longitudinal_velocity_mps, path.points.at(3).point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(3).point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.30, epsilon); + EXPECT_EQ(p.is_final, false); + for (size_t j = 0; j < lane_ids.size(); ++j) { + EXPECT_EQ(lane_ids.at(j), path.points.at(3).lane_ids.at(j)); + } + } + + { + const auto p = resampled_path.points.at(3).point; + const auto lane_ids = resampled_path.points.at(3).lane_ids; + EXPECT_NEAR(p.pose.position.x, 4.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR( + p.longitudinal_velocity_mps, path.points.at(4).point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(4).point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.45, epsilon); + EXPECT_EQ(p.is_final, false); + for (size_t j = 0; j < lane_ids.size(); ++j) { + EXPECT_EQ(lane_ids.at(j), path.points.at(4).lane_ids.at(j)); + } + } + + { + const auto p = resampled_path.points.at(4).point; + const auto lane_ids = resampled_path.points.at(4).lane_ids; + EXPECT_NEAR(p.pose.position.x, 5.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(5).point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.50, epsilon); + EXPECT_EQ(p.is_final, false); + for (size_t j = 0; j < lane_ids.size(); ++j) { + EXPECT_EQ(lane_ids.at(j), path.points.at(5).lane_ids.at(j)); + } + } + + { + const auto p = resampled_path.points.at(5).point; + const auto lane_ids = resampled_path.points.at(5).lane_ids; + EXPECT_NEAR(p.pose.position.x, 6.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR( + p.longitudinal_velocity_mps, path.points.at(6).point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(6).point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.60, epsilon); + EXPECT_EQ(p.is_final, false); + for (size_t j = 0; j < lane_ids.size(); ++j) { + EXPECT_EQ(lane_ids.at(j), path.points.at(6).lane_ids.at(j)); + } + } + + { + const auto p = resampled_path.points.at(6).point; + const auto lane_ids = resampled_path.points.at(6).lane_ids; + EXPECT_NEAR(p.pose.position.x, 7.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR( + p.longitudinal_velocity_mps, path.points.at(7).point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(7).point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon); + EXPECT_EQ(p.is_final, false); + for (size_t j = 0; j < lane_ids.size(); ++j) { + EXPECT_EQ(lane_ids.at(j), path.points.at(7).lane_ids.at(j)); + } + } + + { + const auto p = resampled_path.points.at(7).point; + const auto lane_ids = resampled_path.points.at(7).lane_ids; + EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR( + p.longitudinal_velocity_mps, path.points.at(9).point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(9).point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.90, epsilon); + EXPECT_EQ(p.is_final, true); + for (size_t j = 0; j < lane_ids.size(); ++j) { + EXPECT_EQ(lane_ids.at(j), path.points.at(9).lane_ids.at(j)); + } + } + } + + // No Resample + { + // Input path size is not enough for resample + { + PathWithLaneId path; + path.points.resize(1); + for (size_t i = 0; i < 1; ++i) { + path.points.at(i) = generateTestPathPointWithLaneId( + i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {0}); + } + + const auto resampled_path = resamplePath(path, 1.0); + EXPECT_EQ(resampled_path.points.size(), path.points.size()); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + const auto ans_p = path.points.at(i); + EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon); + EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon); + EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon); + EXPECT_NEAR( + p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); + EXPECT_EQ(p.point.is_final, ans_p.point.is_final); + for (size_t j = 0; j < p.lane_ids.size(); ++j) { + EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j)); + } + } + } + + // Resample interval is invalid + { + PathWithLaneId path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPointWithLaneId( + i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); + } + + const auto resampled_path = resamplePath(path, 1e-4); + EXPECT_EQ(resampled_path.points.size(), path.points.size()); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + const auto ans_p = path.points.at(i); + EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon); + EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon); + EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon); + EXPECT_NEAR( + p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); + EXPECT_EQ(p.point.is_final, ans_p.point.is_final); + for (size_t j = 0; j < p.lane_ids.size(); ++j) { + EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j)); + } + } + } + + // Resample interval is invalid (Negative value) + { + PathWithLaneId path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPointWithLaneId( + i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast(i)}); + } + + const auto resampled_path = resamplePath(path, -5.0); + EXPECT_EQ(resampled_path.points.size(), path.points.size()); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + const auto ans_p = path.points.at(i); + EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon); + EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon); + EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon); + EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon); + EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon); + EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon); + EXPECT_NEAR( + p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon); + EXPECT_EQ(p.point.is_final, ans_p.point.is_final); + for (size_t j = 0; j < p.lane_ids.size(); ++j) { + EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j)); + } + } + } + } +} + +TEST(resample_path, resample_path_by_vector) +{ + using autoware::motion_utils::resamplePath; + // Output is same as input + { + auto path = generateTestPath(10, 1.0, 3.0, 1.0, 0.01); + std::vector resampled_arclength = generateArclength(10, 1.0); + + { + const auto resampled_path = resamplePath(path, resampled_arclength); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + const auto ans_p = path.points.at(i); + EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); + EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); + EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); + EXPECT_NEAR(p.pose.orientation.x, ans_p.pose.orientation.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_p.pose.orientation.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_p.pose.orientation.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_p.pose.orientation.w, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); + } + } + + // Change the last point orientation + path.points.back() = + generateTestPathPoint(9.0, 0.0, 0.0, autoware_utils::pi / 3.0, 3.0, 1.0, 0.01); + { + const auto resampled_path = resamplePath(path, resampled_arclength); + for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { + const auto p = resampled_path.points.at(i); + const auto ans_p = path.points.at(i); + EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); + EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); + EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); + EXPECT_NEAR(p.pose.orientation.x, ans_p.pose.orientation.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_p.pose.orientation.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_p.pose.orientation.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_p.pose.orientation.w, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); + } + + const auto p = resampled_path.points.back(); + const auto ans_p = path.points.back(); + const auto ans_quat = autoware_utils::create_quaternion(0.0, 0.0, 0.0, 1.0); + EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); + EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); + EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); + EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); + } + } + + // Output key is not same as input + { + autoware_planning_msgs::msg::Path path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); + } + std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; + + const auto resampled_path = resamplePath(path, resampled_arclength); + + { + const auto p = resampled_path.points.at(0); + EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); + } + + { + const auto p = resampled_path.points.at(1); + EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); + } + + { + const auto p = resampled_path.points.at(2); + EXPECT_NEAR(p.pose.position.x, 1.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.15, epsilon); + } + + { + const auto p = resampled_path.points.at(3); + EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 5.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); + } + + { + const auto p = resampled_path.points.at(4); + EXPECT_NEAR(p.pose.position.x, 7.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 7.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 3.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon); + } + + { + const auto p = resampled_path.points.at(5); + EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); + } + + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + } + } + + // No Interpolation + { + // Input path size is not enough for interpolation + { + autoware_planning_msgs::msg::Path path; + path.points.resize(1); + for (size_t i = 0; i < 1; ++i) { + path.points.at(i) = + generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); + } + std::vector resampled_arclength = generateArclength(10, 1.0); + + const auto resampled_path = resamplePath(path, resampled_arclength); + EXPECT_EQ(resampled_path.points.size(), path.points.size()); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + EXPECT_NEAR( + resampled_path.points.at(i).pose.position.x, path.points.at(i).pose.position.x, epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.position.y, path.points.at(i).pose.position.y, epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.position.z, path.points.at(i).pose.position.z, epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.x, path.points.at(i).pose.orientation.x, + epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.y, path.points.at(i).pose.orientation.y, + epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.z, path.points.at(i).pose.orientation.z, + epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.w, path.points.at(i).pose.orientation.w, + epsilon); + } + } + + // Resampled Arclength size is not enough for interpolation + { + autoware_planning_msgs::msg::Path path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = + generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); + } + std::vector resampled_arclength = generateArclength(1, 1.0); + + const auto resampled_path = resamplePath(path, resampled_arclength); + EXPECT_EQ(resampled_path.points.size(), path.points.size()); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + EXPECT_NEAR( + resampled_path.points.at(i).pose.position.x, path.points.at(i).pose.position.x, epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.position.y, path.points.at(i).pose.position.y, epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.position.z, path.points.at(i).pose.position.z, epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.x, path.points.at(i).pose.orientation.x, + epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.y, path.points.at(i).pose.orientation.y, + epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.z, path.points.at(i).pose.orientation.z, + epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.w, path.points.at(i).pose.orientation.w, + epsilon); + } + } + + // Resampled Arclength is longer than input path + { + autoware_planning_msgs::msg::Path path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = + generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); + } + std::vector resampled_arclength = generateArclength(3, 5.0); + + const auto resampled_path = resamplePath(path, resampled_arclength); + EXPECT_EQ(resampled_path.points.size(), path.points.size()); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + EXPECT_NEAR( + resampled_path.points.at(i).pose.position.x, path.points.at(i).pose.position.x, epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.position.y, path.points.at(i).pose.position.y, epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.position.z, path.points.at(i).pose.position.z, epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.x, path.points.at(i).pose.orientation.x, + epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.y, path.points.at(i).pose.orientation.y, + epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.z, path.points.at(i).pose.orientation.z, + epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.w, path.points.at(i).pose.orientation.w, + epsilon); + } + } + } +} + +TEST(resample_path, resample_path_by_vector_backward) +{ + using autoware::motion_utils::resamplePath; + + { + autoware_planning_msgs::msg::Path path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1); + } + std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; + + const auto resampled_path = resamplePath(path, resampled_arclength); + + { + const auto p = resampled_path.points.at(0); + EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); + } + + { + const auto p = resampled_path.points.at(1); + EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); + } + + { + const auto p = resampled_path.points.at(2); + EXPECT_NEAR(p.pose.position.x, 1.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.15, epsilon); + } + + { + const auto p = resampled_path.points.at(3); + EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 5.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); + } + + { + const auto p = resampled_path.points.at(4); + EXPECT_NEAR(p.pose.position.x, 7.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 7.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 3.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon); + } + + { + const auto p = resampled_path.points.at(5); + EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); + } + + const auto ans_quat = autoware_utils::create_quaternion_from_yaw(M_PI); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); + } + } + + // change initial orientation + { + autoware_planning_msgs::msg::Path path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1); + } + path.points.at(0).pose.orientation = + autoware_utils::create_quaternion_from_yaw(M_PI + M_PI / 3.0); + std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; + + const auto resampled_path = resamplePath(path, resampled_arclength); + + { + const auto p = resampled_path.points.at(0); + EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); + } + + { + const auto p = resampled_path.points.at(1); + EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); + } + + { + const auto p = resampled_path.points.at(2); + EXPECT_NEAR(p.pose.position.x, 1.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.15, epsilon); + } + + { + const auto p = resampled_path.points.at(3); + EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 5.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); + } + + { + const auto p = resampled_path.points.at(4); + EXPECT_NEAR(p.pose.position.x, 7.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 7.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 3.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon); + } + + { + const auto p = resampled_path.points.at(5); + EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); + } + + // Initial Orientation + { + const auto ans_quat = autoware_utils::create_quaternion_from_yaw(M_PI + M_PI / 3.0); + const auto p = resampled_path.points.at(0); + EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); + } + + const auto ans_quat = autoware_utils::create_quaternion_from_yaw(M_PI); + for (size_t i = 1; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); + } + } +} + +TEST(resample_path, resample_path_by_vector_non_default) +{ + using autoware::motion_utils::resamplePath; + + // Lerp x, y + { + autoware_planning_msgs::msg::Path path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); + } + std::vector resampled_arclength = {0.0, 1.2, 5.3, 9.0}; + + const auto resampled_path = resamplePath(path, resampled_arclength, true); + { + const auto p = resampled_path.points.at(0); + EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); + } + + { + const auto p = resampled_path.points.at(1); + EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); + } + + { + const auto p = resampled_path.points.at(2); + EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 5.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); + } + + { + const auto p = resampled_path.points.at(3); + EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); + } + + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + } + } + + // Slerp z + { + autoware_planning_msgs::msg::Path path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = + generateTestPathPoint(i * 1.0, 0.0, i * 1.0, 0.0, i * 1.0, i * 0.5, i * 0.1); + } + std::vector resampled_arclength = {0.0, 1.2, 5.3, 9.0}; + + const auto resampled_path = resamplePath(path, resampled_arclength, false, false); + { + const auto p = resampled_path.points.at(0); + EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); + } + + { + const auto p = resampled_path.points.at(1); + EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 1.2, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); + } + + { + const auto p = resampled_path.points.at(2); + EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 5.3, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 5.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); + } + + { + const auto p = resampled_path.points.at(3); + EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 9.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); + } + + const double pitch = std::atan(1.0); + const auto ans_quat = autoware_utils::create_quaternion_from_rpy(0.0, pitch, 0.0); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); + } + } + + // Lerp v + { + autoware_planning_msgs::msg::Path path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); + } + std::vector resampled_arclength = {0.0, 1.2, 5.3, 9.0}; + + const auto resampled_path = resamplePath(path, resampled_arclength, false, true, false); + { + const auto p = resampled_path.points.at(0); + EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); + } + + { + const auto p = resampled_path.points.at(1); + EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 1.2, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.6, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); + } + + { + const auto p = resampled_path.points.at(2); + EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 5.3, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 2.65, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); + } + + { + const auto p = resampled_path.points.at(3); + EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); + } + + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + } + } +} + +TEST(resample_path, resample_path_by_same_interval) +{ + using autoware::motion_utils::resamplePath; + + // Same point resampling + { + autoware_planning_msgs::msg::Path path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); + } + + { + const auto resampled_path = resamplePath(path, 1.0); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + const auto ans_p = path.points.at(i); + EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); + EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); + EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); + EXPECT_NEAR(p.pose.orientation.x, ans_p.pose.orientation.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_p.pose.orientation.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_p.pose.orientation.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_p.pose.orientation.w, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); + } + } + + // Change the last point orientation + path.points.back() = + generateTestPathPoint(9.0, 0.0, 0.0, autoware_utils::pi / 3.0, 3.0, 1.0, 0.01); + { + const auto resampled_path = resamplePath(path, 1.0); + for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { + const auto p = resampled_path.points.at(i); + const auto ans_p = path.points.at(i); + EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); + EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); + EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); + EXPECT_NEAR(p.pose.orientation.x, ans_p.pose.orientation.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_p.pose.orientation.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_p.pose.orientation.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_p.pose.orientation.w, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); + } + + const auto p = resampled_path.points.back(); + const auto ans_p = path.points.back(); + const auto ans_quat = autoware_utils::create_quaternion(0.0, 0.0, 0.0, 1.0); + EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); + EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); + EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); + EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); + } + } + + // Normal Case without zero point + { + autoware_planning_msgs::msg::Path path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); + } + path.points.at(0).longitudinal_velocity_mps = 5.0; + + const auto resampled_path = resamplePath(path, 0.1); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + EXPECT_NEAR(p.pose.position.x, 0.1 * i, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + const size_t idx = i / 10; + EXPECT_NEAR( + p.longitudinal_velocity_mps, path.points.at(idx).longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(idx).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.01 * i, epsilon); + } + } + + // Normal Case without stop point but with terminal point + { + autoware_planning_msgs::msg::Path path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); + } + path.points.at(0).longitudinal_velocity_mps = 5.0; + + const auto resampled_path = resamplePath(path, 0.4); + for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { + const auto p = resampled_path.points.at(i); + EXPECT_NEAR(p.pose.position.x, 0.4 * i, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + const size_t idx = i / 2.5; + EXPECT_NEAR( + p.longitudinal_velocity_mps, path.points.at(idx).longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(idx).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.04 * i, epsilon); + } + + { + const auto p = resampled_path.points.at(23); + EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + const size_t idx = 9; + EXPECT_NEAR( + p.longitudinal_velocity_mps, path.points.at(idx).longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(idx).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); + } + } + + // Normal Case without stop point but with terminal point (Boundary Condition) + { + autoware_planning_msgs::msg::Path path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); + } + path.points.at(0).longitudinal_velocity_mps = 5.0; + + const double ds = 1.0 - autoware::motion_utils::overlap_threshold; + const auto resampled_path = resamplePath(path, ds); + for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { + const auto p = resampled_path.points.at(i); + EXPECT_NEAR(p.pose.position.x, ds * i, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + const size_t idx = i == 0 ? 0 : i - 1; + EXPECT_NEAR( + p.longitudinal_velocity_mps, path.points.at(idx).longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(idx).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, ds / 10.0 * i, epsilon); + } + + { + const auto p = resampled_path.points.at(10); + EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + const size_t idx = 9; + EXPECT_NEAR( + p.longitudinal_velocity_mps, path.points.at(idx).longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(idx).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); + } + } + + // Normal Case with duplicated zero point + { + autoware_planning_msgs::msg::Path path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); + } + path.points.at(0).longitudinal_velocity_mps = 5.0; + path.points.at(5).longitudinal_velocity_mps = 0.0; + + const auto resampled_path = resamplePath(path, 0.1); + EXPECT_EQ(resampled_path.points.size(), static_cast(91)); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + EXPECT_NEAR(p.pose.position.x, 0.1 * i, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + const size_t idx = i / 10; + EXPECT_NEAR( + p.longitudinal_velocity_mps, path.points.at(idx).longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(idx).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.01 * i, epsilon); + } + } + + // Normal Case with zero point + { + autoware_planning_msgs::msg::Path path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); + } + path.points.at(0).longitudinal_velocity_mps = 8.0; + path.points.at(5).longitudinal_velocity_mps = 0.0; + + const auto resampled_path = resamplePath(path, 1.5); + EXPECT_EQ(resampled_path.points.size(), static_cast(8)); + { + const auto p = resampled_path.points.at(0); + EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR( + p.longitudinal_velocity_mps, path.points.at(0).longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(0).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); + } + + { + const auto p = resampled_path.points.at(1); + EXPECT_NEAR(p.pose.position.x, 1.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR( + p.longitudinal_velocity_mps, path.points.at(1).longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(1).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.15, epsilon); + } + + { + const auto p = resampled_path.points.at(2); + EXPECT_NEAR(p.pose.position.x, 3.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR( + p.longitudinal_velocity_mps, path.points.at(3).longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(3).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.30, epsilon); + } + + { + const auto p = resampled_path.points.at(3); + EXPECT_NEAR(p.pose.position.x, 4.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR( + p.longitudinal_velocity_mps, path.points.at(4).longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(4).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.45, epsilon); + } + + { + const auto p = resampled_path.points.at(4); + EXPECT_NEAR(p.pose.position.x, 5.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(5).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.50, epsilon); + } + + { + const auto p = resampled_path.points.at(5); + EXPECT_NEAR(p.pose.position.x, 6.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR( + p.longitudinal_velocity_mps, path.points.at(6).longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(6).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.60, epsilon); + } + + { + const auto p = resampled_path.points.at(6); + EXPECT_NEAR(p.pose.position.x, 7.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR( + p.longitudinal_velocity_mps, path.points.at(7).longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(7).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon); + } + + { + const auto p = resampled_path.points.at(7); + EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR( + p.longitudinal_velocity_mps, path.points.at(9).longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(9).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.90, epsilon); + } + } + + // No Resample + { + // Input path size is not enough for resample + { + autoware_planning_msgs::msg::Path path; + path.points.resize(1); + for (size_t i = 0; i < 1; ++i) { + path.points.at(i) = + generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); + } + + const auto resampled_path = resamplePath(path, 1.0); + EXPECT_EQ(resampled_path.points.size(), path.points.size()); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + EXPECT_NEAR( + resampled_path.points.at(i).pose.position.x, path.points.at(i).pose.position.x, epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.position.y, path.points.at(i).pose.position.y, epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.position.z, path.points.at(i).pose.position.z, epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.x, path.points.at(i).pose.orientation.x, + epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.y, path.points.at(i).pose.orientation.y, + epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.z, path.points.at(i).pose.orientation.z, + epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.w, path.points.at(i).pose.orientation.w, + epsilon); + } + } + + // Resample interval is invalid + { + autoware_planning_msgs::msg::Path path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = + generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); + } + + const auto resampled_path = resamplePath(path, 1e-4); + EXPECT_EQ(resampled_path.points.size(), path.points.size()); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + EXPECT_NEAR( + resampled_path.points.at(i).pose.position.x, path.points.at(i).pose.position.x, epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.position.y, path.points.at(i).pose.position.y, epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.position.z, path.points.at(i).pose.position.z, epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.x, path.points.at(i).pose.orientation.x, + epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.y, path.points.at(i).pose.orientation.y, + epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.z, path.points.at(i).pose.orientation.z, + epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.w, path.points.at(i).pose.orientation.w, + epsilon); + } + } + + // Resample interval is invalid (Negative value) + { + autoware_planning_msgs::msg::Path path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = + generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); + } + + const auto resampled_path = resamplePath(path, -5.0); + EXPECT_EQ(resampled_path.points.size(), path.points.size()); + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + EXPECT_NEAR( + resampled_path.points.at(i).pose.position.x, path.points.at(i).pose.position.x, epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.position.y, path.points.at(i).pose.position.y, epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.position.z, path.points.at(i).pose.position.z, epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.x, path.points.at(i).pose.orientation.x, + epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.y, path.points.at(i).pose.orientation.y, + epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.z, path.points.at(i).pose.orientation.z, + epsilon); + EXPECT_NEAR( + resampled_path.points.at(i).pose.orientation.w, path.points.at(i).pose.orientation.w, + epsilon); + } + } + } +} + +TEST(resample_trajectory, resample_trajectory_by_vector) +{ + using autoware::motion_utils::resampleTrajectory; + // Output is same as input + { + auto traj = generateTestTrajectory(10, 1.0, 3.0, 1.0, 0.01, 0.5); + std::vector resampled_arclength = generateArclength(10, 1.0); + + { + const auto resampled_traj = resampleTrajectory(traj, resampled_arclength); + for (size_t i = 0; i < resampled_traj.points.size(); ++i) { + const auto p = resampled_traj.points.at(i); + const auto ans_p = setZeroVelocityAfterStop(traj.points).at(i); + EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); + EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); + EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); + EXPECT_NEAR(p.pose.orientation.x, ans_p.pose.orientation.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_p.pose.orientation.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_p.pose.orientation.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_p.pose.orientation.w, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); + } + } + + // Change the last point orientation + traj.points.back() = + generateTestTrajectoryPoint(9.0, 0.0, 0.0, autoware_utils::pi / 3.0, 3.0, 1.0, 0.01, 0.5); + { + const auto resampled_path = resampleTrajectory(traj, resampled_arclength); + for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { + const auto p = resampled_path.points.at(i); + const auto ans_p = setZeroVelocityAfterStop(traj.points).at(i); + EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); + EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); + EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); + EXPECT_NEAR(p.pose.orientation.x, ans_p.pose.orientation.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_p.pose.orientation.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_p.pose.orientation.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_p.pose.orientation.w, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); + } + + const auto p = resampled_path.points.back(); + const auto ans_p = setZeroVelocityAfterStop(traj.points).back(); + const auto ans_quat = autoware_utils::create_quaternion(0.0, 0.0, 0.0, 1.0); + EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); + EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); + EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); + EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); + } + } + + // Output key is not same as input + { + autoware_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + std::vector resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0}; + + const auto resampled_path = resampleTrajectory(traj, resampled_arclength); + + { + const auto p = resampled_path.points.at(0); + EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); + EXPECT_NEAR(p.acceleration_mps2, 0.0, epsilon); + } + + { + const auto p = resampled_path.points.at(1); + EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); + EXPECT_NEAR(p.acceleration_mps2, 0.05, epsilon); + } + + { + const auto p = resampled_path.points.at(2); + EXPECT_NEAR(p.pose.position.x, 1.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.15, epsilon); + EXPECT_NEAR(p.acceleration_mps2, 0.05, epsilon); + } + + { + const auto p = resampled_path.points.at(3); + EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); + EXPECT_NEAR(p.acceleration_mps2, 0.25, epsilon); + } + + { + const auto p = resampled_path.points.at(4); + EXPECT_NEAR(p.pose.position.x, 7.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 3.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon); + EXPECT_NEAR(p.acceleration_mps2, 0.35, epsilon); + } + + { + const auto p = resampled_path.points.at(5); + EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); + EXPECT_NEAR(p.acceleration_mps2, 0.45, epsilon); + } + + for (size_t i = 0; i < resampled_path.points.size(); ++i) { + const auto p = resampled_path.points.at(i); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + } + } + + // No Interpolation + { + // Input path size is not enough for interpolation + { + autoware_planning_msgs::msg::Trajectory traj; + traj.points.resize(1); + for (size_t i = 0; i < 1; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + std::vector resampled_arclength = generateArclength(10, 1.0); + + const auto resampled_traj = resampleTrajectory(traj, resampled_arclength); + EXPECT_EQ(resampled_traj.points.size(), traj.points.size()); + for (size_t i = 0; i < resampled_traj.points.size(); ++i) { + EXPECT_NEAR( + resampled_traj.points.at(i).pose.position.x, traj.points.at(i).pose.position.x, epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.position.y, traj.points.at(i).pose.position.y, epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.position.z, traj.points.at(i).pose.position.z, epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.x, traj.points.at(i).pose.orientation.x, + epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.y, traj.points.at(i).pose.orientation.y, + epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.z, traj.points.at(i).pose.orientation.z, + epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.w, traj.points.at(i).pose.orientation.w, + epsilon); + } + } + + // Resampled Arclength size is not enough for interpolation + { + autoware_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + std::vector resampled_arclength = generateArclength(1, 1.0); + + const auto resampled_traj = resampleTrajectory(traj, resampled_arclength); + EXPECT_EQ(resampled_traj.points.size(), traj.points.size()); + for (size_t i = 0; i < resampled_traj.points.size(); ++i) { + EXPECT_NEAR( + resampled_traj.points.at(i).pose.position.x, traj.points.at(i).pose.position.x, epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.position.y, traj.points.at(i).pose.position.y, epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.position.z, traj.points.at(i).pose.position.z, epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.x, traj.points.at(i).pose.orientation.x, + epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.y, traj.points.at(i).pose.orientation.y, + epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.z, traj.points.at(i).pose.orientation.z, + epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.w, traj.points.at(i).pose.orientation.w, + epsilon); + } + } + + // Resampled Arclength is longer than input path + { + autoware_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + std::vector resampled_arclength = generateArclength(3, 5.0); + + const auto resampled_traj = resampleTrajectory(traj, resampled_arclength); + EXPECT_EQ(resampled_traj.points.size(), traj.points.size()); + for (size_t i = 0; i < resampled_traj.points.size(); ++i) { + EXPECT_NEAR( + resampled_traj.points.at(i).pose.position.x, traj.points.at(i).pose.position.x, epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.position.y, traj.points.at(i).pose.position.y, epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.position.z, traj.points.at(i).pose.position.z, epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.x, traj.points.at(i).pose.orientation.x, + epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.y, traj.points.at(i).pose.orientation.y, + epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.z, traj.points.at(i).pose.orientation.z, + epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.w, traj.points.at(i).pose.orientation.w, + epsilon); + } + } + } +} + +TEST(resample_trajectory, resample_trajectory_by_vector_non_default) +{ + using autoware::motion_utils::resampleTrajectory; + + // Lerp x, y + { + autoware_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + std::vector resampled_arclength = {0.0, 1.2, 5.3, 9.0}; + + const auto resampled_traj = resampleTrajectory(traj, resampled_arclength, true); + { + const auto p = resampled_traj.points.at(0); + EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); + EXPECT_NEAR(p.acceleration_mps2, 0.0, epsilon); + } + + { + const auto p = resampled_traj.points.at(1); + EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); + EXPECT_NEAR(p.acceleration_mps2, 0.05, epsilon); + } + + { + const auto p = resampled_traj.points.at(2); + EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); + EXPECT_NEAR(p.acceleration_mps2, 0.25, epsilon); + } + + { + const auto p = resampled_traj.points.at(3); + EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); + EXPECT_NEAR(p.acceleration_mps2, 0.45, epsilon); + } + + for (size_t i = 0; i < resampled_traj.points.size(); ++i) { + const auto p = resampled_traj.points.at(i); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + } + } + + // Slerp z + { + autoware_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = generateTestTrajectoryPoint( + i * 1.0, 0.0, i * 1.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + std::vector resampled_arclength = {0.0, 1.2, 5.3, 9.0}; + + const auto resampled_traj = resampleTrajectory(traj, resampled_arclength, false, false); + { + const auto p = resampled_traj.points.at(0); + EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); + EXPECT_NEAR(p.acceleration_mps2, 0.0, epsilon); + } + + { + const auto p = resampled_traj.points.at(1); + EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 1.2, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); + EXPECT_NEAR(p.acceleration_mps2, 0.05, epsilon); + } + + { + const auto p = resampled_traj.points.at(2); + EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 5.3, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 2.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); + EXPECT_NEAR(p.acceleration_mps2, 0.25, epsilon); + } + + { + const auto p = resampled_traj.points.at(3); + EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 9.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); + EXPECT_NEAR(p.acceleration_mps2, 0.45, epsilon); + } + + const double pitch = std::atan(1.0); + const auto ans_quat = autoware_utils::create_quaternion_from_rpy(0.0, pitch, 0.0); + for (size_t i = 0; i < resampled_traj.points.size(); ++i) { + const auto p = resampled_traj.points.at(i); + EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); + } + } + + // Lerp twist + { + autoware_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + std::vector resampled_arclength = {0.0, 1.2, 5.3, 9.0}; + + const auto resampled_traj = resampleTrajectory(traj, resampled_arclength, false, true, false); + { + const auto p = resampled_traj.points.at(0); + EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); + EXPECT_NEAR(p.acceleration_mps2, 0.0, epsilon); + } + + { + const auto p = resampled_traj.points.at(1); + EXPECT_NEAR(p.pose.position.x, 1.2, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 0.6, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.12, epsilon); + EXPECT_NEAR(p.acceleration_mps2, 0.06, epsilon); + } + + { + const auto p = resampled_traj.points.at(2); + EXPECT_NEAR(p.pose.position.x, 5.3, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 2.65, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.53, epsilon); + EXPECT_NEAR(p.acceleration_mps2, 0.265, epsilon); + } + + { + const auto p = resampled_traj.points.at(3); + EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); + EXPECT_NEAR(p.acceleration_mps2, 0.45, epsilon); + } + + for (size_t i = 0; i < resampled_traj.points.size(); ++i) { + const auto p = resampled_traj.points.at(i); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + } + } +} + +TEST(resample_trajectory, resample_trajectory_by_same_interval) +{ + using autoware::motion_utils::resampleTrajectory; + + // Same point resampling + { + autoware_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + + { + const auto resampled_traj = resampleTrajectory(traj, 1.0); + for (size_t i = 0; i < resampled_traj.points.size(); ++i) { + const auto p = resampled_traj.points.at(i); + const auto ans_p = setZeroVelocityAfterStop(traj.points).at(i); + EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); + EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); + EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); + EXPECT_NEAR(p.pose.orientation.x, ans_p.pose.orientation.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_p.pose.orientation.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_p.pose.orientation.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_p.pose.orientation.w, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); + } + } + + // Change the last point orientation + traj.points.back() = + generateTestTrajectoryPoint(9.0, 0.0, 0.0, autoware_utils::pi / 3.0, 3.0, 1.0, 0.01, 0.5); + { + const auto resampled_path = resampleTrajectory(traj, 1.0); + for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) { + const auto p = resampled_path.points.at(i); + const auto ans_p = setZeroVelocityAfterStop(traj.points).at(i); + EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); + EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); + EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); + EXPECT_NEAR(p.pose.orientation.x, ans_p.pose.orientation.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_p.pose.orientation.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_p.pose.orientation.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_p.pose.orientation.w, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); + } + + const auto p = resampled_path.points.back(); + const auto ans_p = setZeroVelocityAfterStop(traj.points).back(); + const auto ans_quat = autoware_utils::create_quaternion(0.0, 0.0, 0.0, 1.0); + EXPECT_NEAR(p.pose.position.x, ans_p.pose.position.x, epsilon); + EXPECT_NEAR(p.pose.position.y, ans_p.pose.position.y, epsilon); + EXPECT_NEAR(p.pose.position.z, ans_p.pose.position.z, epsilon); + EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, ans_p.heading_rate_rps, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); + } + } + + // Normal Case without zero point + { + autoware_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + traj.points.at(0).longitudinal_velocity_mps = 5.0; + + const auto resampled_traj = resampleTrajectory(traj, 0.1); + for (size_t i = 0; i < resampled_traj.points.size(); ++i) { + const auto p = resampled_traj.points.at(i); + EXPECT_NEAR(p.pose.position.x, 0.1 * i, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + const size_t idx = i / 10; + auto ans_p = setZeroVelocityAfterStop(traj.points).at(idx); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.01 * i, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); + } + } + + // Normal Case without stop point but with terminal point + { + autoware_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + traj.points.at(0).longitudinal_velocity_mps = 5.0; + + const auto resampled_traj = resampleTrajectory(traj, 0.4); + for (size_t i = 0; i < resampled_traj.points.size() - 1; ++i) { + const auto p = resampled_traj.points.at(i); + EXPECT_NEAR(p.pose.position.x, 0.4 * i, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + const size_t idx = i / 2.5; + auto ans_p = setZeroVelocityAfterStop(traj.points).at(idx); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.04 * i, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); + } + + { + const auto p = resampled_traj.points.at(23); + EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + const size_t idx = 9; + auto ans_p = setZeroVelocityAfterStop(traj.points).at(idx); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); + } + } + + // Normal Case without stop point but with terminal point (Boundary Condition) + { + autoware_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + traj.points.at(0).longitudinal_velocity_mps = 5.0; + + const double ds = 1.0 - autoware::motion_utils::overlap_threshold; + const auto resampled_traj = resampleTrajectory(traj, ds); + for (size_t i = 0; i < resampled_traj.points.size() - 1; ++i) { + const auto p = resampled_traj.points.at(i); + EXPECT_NEAR(p.pose.position.x, ds * i, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + const size_t idx = i == 0 ? 0 : i - 1; + auto ans_p = setZeroVelocityAfterStop(traj.points).at(idx); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, ds / 10.0 * i, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); + } + + { + const auto p = resampled_traj.points.at(10); + EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + const size_t idx = 9; + auto ans_p = setZeroVelocityAfterStop(traj.points).at(idx); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.9, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); + } + } + + // Normal Case with duplicated zero point + { + autoware_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + traj.points.at(0).longitudinal_velocity_mps = 5.0; + traj.points.at(5).longitudinal_velocity_mps = 0.0; + + const auto resampled_traj = resampleTrajectory(traj, 0.1); + EXPECT_EQ(resampled_traj.points.size(), static_cast(91)); + for (size_t i = 0; i < resampled_traj.points.size(); ++i) { + const auto p = resampled_traj.points.at(i); + EXPECT_NEAR(p.pose.position.x, 0.1 * i, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + const size_t idx = i / 10; + auto ans_p = setZeroVelocityAfterStop(traj.points).at(idx); + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.01 * i, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); + } + } + + // Normal Case with zero point + { + autoware_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + traj.points.at(0).longitudinal_velocity_mps = 8.0; + traj.points.at(5).longitudinal_velocity_mps = 0.0; + + const auto resampled_traj = resampleTrajectory(traj, 1.5); + EXPECT_EQ(resampled_traj.points.size(), static_cast(8)); + { + const auto p = resampled_traj.points.at(0); + EXPECT_NEAR(p.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR( + p.longitudinal_velocity_mps, traj.points.at(0).longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, traj.points.at(0).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon); + EXPECT_NEAR(p.acceleration_mps2, traj.points.at(0).acceleration_mps2, epsilon); + } + + { + const auto p = resampled_traj.points.at(1); + EXPECT_NEAR(p.pose.position.x, 1.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR( + p.longitudinal_velocity_mps, traj.points.at(1).longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, traj.points.at(1).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.15, epsilon); + EXPECT_NEAR(p.acceleration_mps2, traj.points.at(1).acceleration_mps2, epsilon); + } + + { + const auto p = resampled_traj.points.at(2); + EXPECT_NEAR(p.pose.position.x, 3.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR( + p.longitudinal_velocity_mps, traj.points.at(3).longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, traj.points.at(3).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.30, epsilon); + EXPECT_NEAR(p.acceleration_mps2, traj.points.at(3).acceleration_mps2, epsilon); + } + + { + const auto p = resampled_traj.points.at(3); + EXPECT_NEAR(p.pose.position.x, 4.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR( + p.longitudinal_velocity_mps, traj.points.at(4).longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, traj.points.at(4).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.45, epsilon); + EXPECT_NEAR(p.acceleration_mps2, traj.points.at(4).acceleration_mps2, epsilon); + } + + { + const auto p = resampled_traj.points.at(4); + EXPECT_NEAR(p.pose.position.x, 5.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, traj.points.at(5).lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.50, epsilon); + EXPECT_NEAR(p.acceleration_mps2, traj.points.at(5).acceleration_mps2, epsilon); + } + + { + const auto p = resampled_traj.points.at(5); + auto ans_p = setZeroVelocityAfterStop(traj.points).at(6); + EXPECT_NEAR(p.pose.position.x, 6.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.60, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); + } + + { + const auto p = resampled_traj.points.at(6); + auto ans_p = setZeroVelocityAfterStop(traj.points).at(7); + EXPECT_NEAR(p.pose.position.x, 7.5, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); + } + + { + const auto p = resampled_traj.points.at(7); + const auto ans_p = setZeroVelocityAfterStop(traj.points).at(9); + EXPECT_NEAR(p.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(p.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon); + + EXPECT_NEAR(p.longitudinal_velocity_mps, ans_p.longitudinal_velocity_mps, epsilon); + EXPECT_NEAR(p.lateral_velocity_mps, ans_p.lateral_velocity_mps, epsilon); + EXPECT_NEAR(p.heading_rate_rps, 0.90, epsilon); + EXPECT_NEAR(p.acceleration_mps2, ans_p.acceleration_mps2, epsilon); + } + } + + // No Resample + { + // Input path size is not enough for resample + { + autoware_planning_msgs::msg::Trajectory traj; + traj.points.resize(1); + for (size_t i = 0; i < 1; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + + const auto resampled_traj = resampleTrajectory(traj, 1.0); + EXPECT_EQ(resampled_traj.points.size(), traj.points.size()); + for (size_t i = 0; i < resampled_traj.points.size(); ++i) { + EXPECT_NEAR( + resampled_traj.points.at(i).pose.position.x, traj.points.at(i).pose.position.x, epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.position.y, traj.points.at(i).pose.position.y, epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.position.z, traj.points.at(i).pose.position.z, epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.x, traj.points.at(i).pose.orientation.x, + epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.y, traj.points.at(i).pose.orientation.y, + epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.z, traj.points.at(i).pose.orientation.z, + epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.w, traj.points.at(i).pose.orientation.w, + epsilon); + } + } + + // Resample interval is invalid + { + autoware_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + + const auto resampled_traj = resampleTrajectory(traj, 1e-4); + EXPECT_EQ(resampled_traj.points.size(), traj.points.size()); + for (size_t i = 0; i < resampled_traj.points.size(); ++i) { + EXPECT_NEAR( + resampled_traj.points.at(i).pose.position.x, traj.points.at(i).pose.position.x, epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.position.y, traj.points.at(i).pose.position.y, epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.position.z, traj.points.at(i).pose.position.z, epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.x, traj.points.at(i).pose.orientation.x, + epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.y, traj.points.at(i).pose.orientation.y, + epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.z, traj.points.at(i).pose.orientation.z, + epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.w, traj.points.at(i).pose.orientation.w, + epsilon); + } + } + + // Resample interval is invalid (Negative value) + { + autoware_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + + const auto resampled_traj = resampleTrajectory(traj, -5.0); + EXPECT_EQ(resampled_traj.points.size(), traj.points.size()); + for (size_t i = 0; i < resampled_traj.points.size(); ++i) { + EXPECT_NEAR( + resampled_traj.points.at(i).pose.position.x, traj.points.at(i).pose.position.x, epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.position.y, traj.points.at(i).pose.position.y, epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.position.z, traj.points.at(i).pose.position.z, epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.x, traj.points.at(i).pose.orientation.x, + epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.y, traj.points.at(i).pose.orientation.y, + epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.z, traj.points.at(i).pose.orientation.z, + epsilon); + EXPECT_NEAR( + resampled_traj.points.at(i).pose.orientation.w, traj.points.at(i).pose.orientation.w, + epsilon); + } + } + } +} + +TEST(resample_trajectory, resample_with_middle_stop_point) +{ + // This test is to check the behavior when the stop point is unstably resampled by zero-order hold + // interpolation. + + using autoware::motion_utils::resampleTrajectory; + + autoware_planning_msgs::msg::Trajectory traj; + traj.points.reserve(10); + + traj.points.push_back(generateTestTrajectoryPoint(0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0)); + traj.points.push_back(generateTestTrajectoryPoint(1.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0)); + traj.points.push_back(generateTestTrajectoryPoint(2.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0)); + traj.points.push_back(generateTestTrajectoryPoint(3.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + traj.points.push_back(generateTestTrajectoryPoint(3.1, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0)); + traj.points.push_back(generateTestTrajectoryPoint(4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + traj.points.push_back(generateTestTrajectoryPoint(5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + traj.points.push_back(generateTestTrajectoryPoint(6.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + traj.points.push_back(generateTestTrajectoryPoint(7.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + + std::vector interpolated_axis = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0}; + + const auto resampled_traj = resampleTrajectory(traj, interpolated_axis); + + EXPECT_NEAR(resampled_traj.points.at(0).longitudinal_velocity_mps, 10.0, epsilon); + EXPECT_NEAR(resampled_traj.points.at(1).longitudinal_velocity_mps, 10.0, epsilon); + EXPECT_NEAR(resampled_traj.points.at(2).longitudinal_velocity_mps, 10.0, epsilon); + EXPECT_NEAR(resampled_traj.points.at(3).longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(resampled_traj.points.at(4).longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(resampled_traj.points.at(5).longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(resampled_traj.points.at(6).longitudinal_velocity_mps, 0.0, epsilon); +} diff --git a/common/autoware_motion_utils/test/src/test_motion_utils.cpp b/common/autoware_motion_utils/test/src/test_motion_utils.cpp new file mode 100644 index 00000000..d8b9b0ac --- /dev/null +++ b/common/autoware_motion_utils/test/src/test_motion_utils.cpp @@ -0,0 +1,26 @@ +// Copyright 2022 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 + +#include + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp b/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp new file mode 100644 index 00000000..f40159d7 --- /dev/null +++ b/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp @@ -0,0 +1,75 @@ +// 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/motion_utils/trajectory/trajectory.hpp" + +#include +#include +#include + +#include + +namespace +{ +using autoware_planning_msgs::msg::Trajectory; +using autoware_utils::create_point; +using autoware_utils::create_quaternion_from_rpy; + +geometry_msgs::msg::Pose createPose( + double x, double y, double z, double roll, double pitch, double yaw) +{ + geometry_msgs::msg::Pose p; + p.position = create_point(x, y, z); + p.orientation = create_quaternion_from_rpy(roll, pitch, yaw); + return p; +} + +template +T generateTestTrajectory( + const size_t num_points, const double point_interval, const double vel = 0.0, + const double init_theta = 0.0, const double delta_theta = 0.0) +{ + using Point = typename T::_points_type::value_type; + + T traj; + for (size_t i = 0; i < num_points; ++i) { + const double theta = init_theta + i * delta_theta; + const double x = i * point_interval * std::cos(theta); + const double y = i * point_interval * std::sin(theta); + + Point p; + p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); + p.longitudinal_velocity_mps = vel; + traj.points.push_back(p); + } + + return traj; +} +} // namespace + +TEST(trajectory_benchmark, DISABLED_calcLateralOffset) +{ + std::random_device r; + std::default_random_engine e1(r()); + std::uniform_real_distribution uniform_dist(0.0, 1000.0); + + using autoware::motion_utils::calcLateralOffset; + + const auto traj = generateTestTrajectory(1000, 1.0, 0.0, 0.0, 0.1); + constexpr auto nb_iteration = 10000; + for (auto i = 0; i < nb_iteration; ++i) { + const auto point = create_point(uniform_dist(e1), uniform_dist(e1), 0.0); + calcLateralOffset(traj.points, point); + } +} diff --git a/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp b/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp new file mode 100644 index 00000000..7d05d69d --- /dev/null +++ b/common/autoware_motion_utils/test/src/trajectory/test_interpolation.cpp @@ -0,0 +1,664 @@ +// 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/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware_utils/geometry/boost_geometry.hpp" +#include "autoware_utils/math/unit_conversion.hpp" + +#include +#include +#include + +#include +#include + +namespace +{ +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_utils::create_point; +using autoware_utils::create_quaternion_from_rpy; +using autoware_utils::transform_point; + +constexpr double epsilon = 1e-6; + +geometry_msgs::msg::Pose createPose( + double x, double y, double z, double roll, double pitch, double yaw) +{ + geometry_msgs::msg::Pose p; + p.position = create_point(x, y, z); + p.orientation = create_quaternion_from_rpy(roll, pitch, yaw); + return p; +} + +TrajectoryPoint generateTestTrajectoryPoint( + const double x, const double y, const double z, const double theta = 0.0, + const double vel_lon = 0.0, const double vel_lat = 0.0, const double heading_rate = 0.0, + const double acc = 0.0) +{ + TrajectoryPoint p; + p.pose = createPose(x, y, z, 0.0, 0.0, theta); + p.longitudinal_velocity_mps = vel_lon; + p.lateral_velocity_mps = vel_lat; + p.heading_rate_rps = heading_rate; + p.acceleration_mps2 = acc; + return p; +} + +template +T generateTestTrajectory( + const size_t num_points, const double point_interval, const double vel_lon = 0.0, + const double vel_lat = 0.0, const double heading_rate_rps = 0.0, const double acc = 0.0, + const double init_theta = 0.0, const double delta_theta = 0.0) +{ + using Point = typename T::_points_type::value_type; + + T traj; + for (size_t i = 0; i < num_points; ++i) { + const double theta = init_theta + i * delta_theta; + const double x = i * point_interval * std::cos(theta); + const double y = i * point_interval * std::sin(theta); + + Point p; + p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); + p.longitudinal_velocity_mps = vel_lon; + p.lateral_velocity_mps = vel_lat; + p.heading_rate_rps = heading_rate_rps; + p.acceleration_mps2 = acc; + traj.points.push_back(p); + } + return traj; +} + +PathPointWithLaneId generateTestPathPoint( + const double x, const double y, const double z, const double theta = 0.0, + const double vel_lon = 0.0, const double vel_lat = 0.0, const double heading_rate = 0.0) +{ + PathPointWithLaneId p; + p.point.pose = createPose(x, y, z, 0.0, 0.0, theta); + p.point.longitudinal_velocity_mps = vel_lon; + p.point.lateral_velocity_mps = vel_lat; + p.point.heading_rate_rps = heading_rate; + return p; +} + +template +T generateTestPath( + const size_t num_points, const double point_interval, const double vel_lon = 0.0, + const double vel_lat = 0.0, const double heading_rate_rps = 0.0, const double init_theta = 0.0, + const double delta_theta = 0.0) +{ + using Point = typename T::_points_type::value_type; + + T traj; + for (size_t i = 0; i < num_points; ++i) { + const double theta = init_theta + i * delta_theta; + const double x = i * point_interval * std::cos(theta); + const double y = i * point_interval * std::sin(theta); + + Point p; + p.point.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); + p.point.longitudinal_velocity_mps = vel_lon; + p.point.lateral_velocity_mps = vel_lat; + p.point.heading_rate_rps = heading_rate_rps; + traj.points.push_back(p); + } + return traj; +} +} // namespace + +TEST(Interpolation, interpolate_path_for_trajectory) +{ + using autoware::motion_utils::calcInterpolatedPoint; + + { + autoware_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + + // Same points as the trajectory point + { + const auto target_pose = createPose(3.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 3.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 3.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 1.5, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.3, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.15, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + + // Random Point + { + const auto target_pose = createPose(2.5, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 2.5, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 2.5, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 1.25, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.25, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.125, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + + // Random Point with zero order hold + { + const auto target_pose = createPose(2.5, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose, true); + + EXPECT_NEAR(result.pose.position.x, 2.5, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 2.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.25, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.1, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + + // Initial Point + { + const auto target_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.0, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.0, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + + // Terminal Point + { + const auto target_pose = createPose(9.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.9, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.45, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + + // Outside of initial point + { + const auto target_pose = createPose(-2.0, -9.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.0, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.0, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + + // Outside of terminal point + { + const auto target_pose = createPose(10.0, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.9, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.45, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + } + + // Empty Point + { + const Trajectory traj; + const auto target_pose = createPose(3.0, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 3.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 5.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.0, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.0, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + + // One point + { + const auto traj = generateTestTrajectory(1, 1.0, 1.0, 0.5, 0.1, 0.05); + const auto target_pose = createPose(3.0, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.1, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.05, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } + + // Duplicated Points + { + autoware_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + traj.points.at(4) = traj.points.at(3); + + const auto target_pose = createPose(3.2, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(traj, target_pose); + + EXPECT_NEAR(result.pose.position.x, 3.0, epsilon); + EXPECT_NEAR(result.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.longitudinal_velocity_mps, 3.0, epsilon); + EXPECT_NEAR(result.lateral_velocity_mps, 1.5, epsilon); + EXPECT_NEAR(result.heading_rate_rps, 0.3, epsilon); + EXPECT_NEAR(result.acceleration_mps2, 0.15, epsilon); + EXPECT_NEAR(result.front_wheel_angle_rad, 0.0, epsilon); + EXPECT_NEAR(result.rear_wheel_angle_rad, 0.0, epsilon); + } +} + +TEST(Interpolation, interpolate_path_for_path) +{ + using autoware::motion_utils::calcInterpolatedPoint; + + { + autoware_internal_planning_msgs::msg::PathWithLaneId path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); + } + + // Same points as the path point + { + const auto target_pose = createPose(3.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(path, target_pose); + + EXPECT_NEAR(result.point.pose.position.x, 3.0, epsilon); + EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.point.longitudinal_velocity_mps, 3.0, epsilon); + EXPECT_NEAR(result.point.lateral_velocity_mps, 1.5, epsilon); + EXPECT_NEAR(result.point.heading_rate_rps, 0.3, epsilon); + } + + // Random Point + { + const auto target_pose = createPose(2.5, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(path, target_pose); + + EXPECT_NEAR(result.point.pose.position.x, 2.5, epsilon); + EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.point.longitudinal_velocity_mps, 2.5, epsilon); + EXPECT_NEAR(result.point.lateral_velocity_mps, 1.25, epsilon); + EXPECT_NEAR(result.point.heading_rate_rps, 0.25, epsilon); + } + + // Random Point with zero order hold + { + const auto target_pose = createPose(2.5, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(path, target_pose, true); + + EXPECT_NEAR(result.point.pose.position.x, 2.5, epsilon); + EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.point.longitudinal_velocity_mps, 2.0, epsilon); + EXPECT_NEAR(result.point.lateral_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(result.point.heading_rate_rps, 0.25, epsilon); + } + + // Initial Point + { + const auto target_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(path, target_pose); + + EXPECT_NEAR(result.point.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.point.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.point.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.point.heading_rate_rps, 0.0, epsilon); + } + + // Terminal Point + { + const auto target_pose = createPose(9.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(path, target_pose); + + EXPECT_NEAR(result.point.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.point.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(result.point.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(result.point.heading_rate_rps, 0.9, epsilon); + } + + // Outside of initial point + { + const auto target_pose = createPose(-2.0, -9.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(path, target_pose); + + EXPECT_NEAR(result.point.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.point.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.point.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.point.heading_rate_rps, 0.0, epsilon); + } + + // Outside of terminal point + { + const auto target_pose = createPose(10.0, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(path, target_pose); + + EXPECT_NEAR(result.point.pose.position.x, 9.0, epsilon); + EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.point.longitudinal_velocity_mps, 9.0, epsilon); + EXPECT_NEAR(result.point.lateral_velocity_mps, 4.5, epsilon); + EXPECT_NEAR(result.point.heading_rate_rps, 0.9, epsilon); + } + } + + // Empty Point + { + const PathWithLaneId path; + const auto target_pose = createPose(3.0, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(path, target_pose); + + EXPECT_NEAR(result.point.pose.position.x, 3.0, epsilon); + EXPECT_NEAR(result.point.pose.position.y, 5.0, epsilon); + EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.point.longitudinal_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.point.lateral_velocity_mps, 0.0, epsilon); + EXPECT_NEAR(result.point.heading_rate_rps, 0.0, epsilon); + } + + // One point + { + const auto path = generateTestPath(1, 1.0, 1.0, 0.5, 0.1); + const auto target_pose = createPose(3.0, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(path, target_pose); + + EXPECT_NEAR(result.point.pose.position.x, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.point.longitudinal_velocity_mps, 1.0, epsilon); + EXPECT_NEAR(result.point.lateral_velocity_mps, 0.5, epsilon); + EXPECT_NEAR(result.point.heading_rate_rps, 0.1, epsilon); + } + + // Duplicated Points + { + autoware_internal_planning_msgs::msg::PathWithLaneId path; + path.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); + } + path.points.at(4) = path.points.at(3); + + const auto target_pose = createPose(3.2, 5.0, 0.0, 0.0, 0.0, 0.0); + const auto result = calcInterpolatedPoint(path, target_pose); + + EXPECT_NEAR(result.point.pose.position.x, 3.0, epsilon); + EXPECT_NEAR(result.point.pose.position.y, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.position.z, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.point.pose.orientation.w, 1.0, epsilon); + EXPECT_NEAR(result.point.longitudinal_velocity_mps, 3.0, epsilon); + EXPECT_NEAR(result.point.lateral_velocity_mps, 1.5, epsilon); + EXPECT_NEAR(result.point.heading_rate_rps, 0.3, epsilon); + } +} + +TEST(Interpolation, interpolate_points_with_length) +{ + using autoware::motion_utils::calcInterpolatedPose; + + { + autoware_planning_msgs::msg::Trajectory traj; + traj.points.resize(10); + for (size_t i = 0; i < 10; ++i) { + traj.points.at(i) = + generateTestTrajectoryPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + + // Same points as the trajectory point + { + const auto result = calcInterpolatedPose(traj.points, 3.0); + + EXPECT_NEAR(result.position.x, 3.0, epsilon); + EXPECT_NEAR(result.position.y, 0.0, epsilon); + EXPECT_NEAR(result.position.z, 0.0, epsilon); + EXPECT_NEAR(result.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.orientation.w, 1.0, epsilon); + } + + // Random Point + { + const auto result = calcInterpolatedPose(traj.points, 2.5); + + EXPECT_NEAR(result.position.x, 2.5, epsilon); + EXPECT_NEAR(result.position.y, 0.0, epsilon); + EXPECT_NEAR(result.position.z, 0.0, epsilon); + EXPECT_NEAR(result.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.orientation.w, 1.0, epsilon); + } + + // Negative length + { + const auto result = calcInterpolatedPose(traj.points, -3.0); + + EXPECT_NEAR(result.position.x, 0.0, epsilon); + EXPECT_NEAR(result.position.y, 0.0, epsilon); + EXPECT_NEAR(result.position.z, 0.0, epsilon); + EXPECT_NEAR(result.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.orientation.w, 1.0, epsilon); + } + + // Boundary value (0.0) + { + const auto result = calcInterpolatedPose(traj.points, 0.0); + + EXPECT_NEAR(result.position.x, 0.0, epsilon); + EXPECT_NEAR(result.position.y, 0.0, epsilon); + EXPECT_NEAR(result.position.z, 0.0, epsilon); + EXPECT_NEAR(result.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.orientation.w, 1.0, epsilon); + } + + // Terminal Point + { + const auto result = calcInterpolatedPose(traj.points, 9.0); + + EXPECT_NEAR(result.position.x, 9.0, epsilon); + EXPECT_NEAR(result.position.y, 0.0, epsilon); + EXPECT_NEAR(result.position.z, 0.0, epsilon); + EXPECT_NEAR(result.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.orientation.w, 1.0, epsilon); + } + + // Outside of terminal point + { + const auto result = calcInterpolatedPose(traj.points, 10.0); + + EXPECT_NEAR(result.position.x, 9.0, epsilon); + EXPECT_NEAR(result.position.y, 0.0, epsilon); + EXPECT_NEAR(result.position.z, 0.0, epsilon); + EXPECT_NEAR(result.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.orientation.w, 1.0, epsilon); + } + } + + // one point + { + autoware_planning_msgs::msg::Trajectory traj; + traj.points.resize(1); + for (size_t i = 0; i < 1; ++i) { + traj.points.at(i) = generateTestTrajectoryPoint( + (i + 1) * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, i * 0.05); + } + + const auto result = calcInterpolatedPose(traj.points, 2.0); + EXPECT_NEAR(result.position.x, 1.0, epsilon); + EXPECT_NEAR(result.position.y, 0.0, epsilon); + EXPECT_NEAR(result.position.z, 0.0, epsilon); + EXPECT_NEAR(result.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.orientation.w, 1.0, epsilon); + } + + // Empty Point + { + const Trajectory traj; + const auto result = calcInterpolatedPose(traj.points, 2.0); + + EXPECT_NEAR(result.position.x, 0.0, epsilon); + EXPECT_NEAR(result.position.y, 0.0, epsilon); + EXPECT_NEAR(result.position.z, 0.0, epsilon); + EXPECT_NEAR(result.orientation.x, 0.0, epsilon); + EXPECT_NEAR(result.orientation.y, 0.0, epsilon); + EXPECT_NEAR(result.orientation.z, 0.0, epsilon); + EXPECT_NEAR(result.orientation.w, 1.0, epsilon); + } +} diff --git a/common/autoware_motion_utils/test/src/trajectory/test_path_shift.cpp b/common/autoware_motion_utils/test/src/trajectory/test_path_shift.cpp new file mode 100644 index 00000000..fbdcc7a8 --- /dev/null +++ b/common/autoware_motion_utils/test/src/trajectory/test_path_shift.cpp @@ -0,0 +1,163 @@ +// Copyright 2024 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/motion_utils/trajectory/path_shift.hpp" + +#include + +TEST(path_shift_test, calc_feasible_velocity_from_jerk) +{ + using autoware::motion_utils::calc_feasible_velocity_from_jerk; + + double longitudinal_distance = 0.0; + double lateral_distance = 0.0; + double lateral_jerk = 0.0; + + // Condition: zero lateral jerk + EXPECT_DOUBLE_EQ( + calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), + 1.0e10); + + // Condition: zero lateral distance + lateral_jerk = 1.0; + EXPECT_DOUBLE_EQ( + calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), + 1.0e10); + + // Condition: zero longitudinal distance + lateral_distance = 2.0; + EXPECT_DOUBLE_EQ( + calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), 0.0); + + // Condition: random condition + longitudinal_distance = 50.0; + EXPECT_DOUBLE_EQ( + calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), 12.5); +} + +TEST(path_shift_test, calc_lateral_dist_from_jerk) +{ + using autoware::motion_utils::calc_lateral_dist_from_jerk; + + double longitudinal_distance = 0.0; + double lateral_jerk = 0.0; + double velocity = 0.0; + + // Condition: zero lateral jerk + EXPECT_DOUBLE_EQ( + calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 1.0e10); + + // Condition: zero velocity + lateral_jerk = 2.0; + EXPECT_DOUBLE_EQ( + calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 1.0e10); + + // Condition: zero longitudinal distance + velocity = 10.0; + EXPECT_DOUBLE_EQ(calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 0.0); + + // Condition: random condition + longitudinal_distance = 100.0; + EXPECT_DOUBLE_EQ( + calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 62.5); +} + +TEST(path_shift_test, calc_longitudinal_dist_from_jerk) +{ + using autoware::motion_utils::calc_longitudinal_dist_from_jerk; + + double lateral_distance = 0.0; + double lateral_jerk = 0.0; + double velocity = 0.0; + + // Condition: zero lateral jerk + EXPECT_DOUBLE_EQ( + calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 1.0e10); + + // Condition: zero lateral distance + lateral_jerk = -1.0; + velocity = 10.0; + EXPECT_DOUBLE_EQ(calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 0.0); + + // Condition: zero velocity + velocity = 0.0; + lateral_distance = 54.0; + EXPECT_DOUBLE_EQ(calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 0.0); + + // Condition: random + velocity = 8.0; + EXPECT_DOUBLE_EQ( + calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 96.0); +} + +TEST(path_shift_test, calc_shift_time_from_jerk) +{ + constexpr double epsilon = 1e-6; + + using autoware::motion_utils::calc_shift_time_from_jerk; + + double lateral_distance = 0.0; + double lateral_jerk = 0.0; + double lateral_acceleration = 0.0; + + // Condition: zero lateral jerk + EXPECT_DOUBLE_EQ( + calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 1.0e10); + + // Condition: zero lateral acceleration + lateral_jerk = -2.0; + EXPECT_DOUBLE_EQ( + calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 1.0e10); + + // Condition: zero lateral distance + lateral_acceleration = -4.0; + EXPECT_DOUBLE_EQ( + calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 0.0); + + // Condition: random (TODO: use DOUBLE_EQ in future. currently not precise enough) + lateral_distance = 80.0; + EXPECT_NEAR( + calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 11.16515139, + epsilon); +} + +TEST(path_shift_test, calc_jerk_from_lat_lon_distance) +{ + using autoware::motion_utils::calc_jerk_from_lat_lon_distance; + + double lateral_distance = 0.0; + double longitudinal_distance = 100.0; + double velocity = 10.0; + + // Condition: zero lateral distance + EXPECT_DOUBLE_EQ( + calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.0); + + // Condition: zero velocity + lateral_distance = 5.0; + velocity = 0.0; + EXPECT_DOUBLE_EQ( + calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.0); + + // Condition: zero longitudinal distance + longitudinal_distance = 0.0; + velocity = 10.0; + EXPECT_DOUBLE_EQ( + calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 1.6 * 1e14); + + // Condition: random + longitudinal_distance = 100.0; + EXPECT_DOUBLE_EQ( + calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.16); +} diff --git a/common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp new file mode 100644 index 00000000..1315a0c6 --- /dev/null +++ b/common/autoware_motion_utils/test/src/trajectory/test_path_with_lane_id.cpp @@ -0,0 +1,190 @@ +// 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/motion_utils/trajectory/path_with_lane_id.hpp" +#include "autoware_utils/geometry/geometry.hpp" + +#include + +#include +#include + +namespace +{ +using autoware_internal_planning_msgs::msg::PathPointWithLaneId; +using autoware_internal_planning_msgs::msg::PathWithLaneId; +using autoware_utils::create_point; + +geometry_msgs::msg::Pose createPose( + double x, double y, double z, double roll, double pitch, double yaw) +{ + geometry_msgs::msg::Pose p; + p.position = create_point(x, y, z); + p.orientation = autoware_utils::create_quaternion_from_rpy(roll, pitch, yaw); + return p; +} + +PathWithLaneId generateTestPathWithLaneId(const size_t num_points, const double point_interval) +{ + PathWithLaneId path; + for (size_t i = 0; i < num_points; ++i) { + const double x = i * point_interval; + const double y = 0.0; + + PathPointWithLaneId p; + p.point.pose = createPose(x, y, 0.0, 0.0, 0.0, 0.0); + p.lane_ids.push_back(i); + path.points.push_back(p); + } + + return path; +} +} // namespace + +TEST(path_with_lane_id, getPathIndexRangeWithLaneId) +{ + using autoware::motion_utils::getPathIndexRangeWithLaneId; + using autoware_internal_planning_msgs::msg::PathWithLaneId; + + // Usual cases + { + PathWithLaneId points; + points.points.resize(6); + points.points.at(0).lane_ids.push_back(3); + points.points.at(1).lane_ids.push_back(3); + points.points.at(2).lane_ids.push_back(1); + points.points.at(3).lane_ids.push_back(2); + points.points.at(4).lane_ids.push_back(2); + points.points.at(5).lane_ids.push_back(2); + + { + const auto res = getPathIndexRangeWithLaneId(points, 3); + EXPECT_EQ(res->first, 0U); + EXPECT_EQ(res->second, 2U); + } + { + const auto res = getPathIndexRangeWithLaneId(points, 1); + EXPECT_EQ(res->first, 1U); + EXPECT_EQ(res->second, 3U); + } + { + const auto res = getPathIndexRangeWithLaneId(points, 2); + EXPECT_EQ(res->first, 2U); + EXPECT_EQ(res->second, 5U); + } + { + const auto res = getPathIndexRangeWithLaneId(points, 4); + EXPECT_EQ(res, std::nullopt); + } + } + + // Empty points + { + PathWithLaneId points; + const auto res = getPathIndexRangeWithLaneId(points, 4); + EXPECT_EQ(res, std::nullopt); + } +} + +TEST(path_with_lane_id, findNearestIndexFromLaneId) +{ + using autoware::motion_utils::findNearestIndexFromLaneId; + using autoware::motion_utils::findNearestSegmentIndexFromLaneId; + + const auto path = generateTestPathWithLaneId(10, 1.0); + + // Normal cases + { + auto modified_path = path; + for (size_t i = 0; i < 10; ++i) { + modified_path.points.at(i).lane_ids = {100}; + } + EXPECT_EQ(findNearestIndexFromLaneId(modified_path, create_point(2.4, 1.3, 0.0), 100), 2U); + EXPECT_EQ( + findNearestSegmentIndexFromLaneId(modified_path, create_point(2.4, 1.3, 0.0), 100), 2U); + } + + { + auto modified_path = path; + for (size_t i = 3; i < 6; ++i) { + modified_path.points.at(i).lane_ids = {100}; + } + EXPECT_EQ(findNearestIndexFromLaneId(modified_path, create_point(4.1, 0.3, 0.0), 100), 4U); + EXPECT_EQ( + findNearestSegmentIndexFromLaneId(modified_path, create_point(4.1, 0.3, 0.0), 100), 4U); + } + + { + auto modified_path = path; + for (size_t i = 8; i < 9; ++i) { + modified_path.points.at(i).lane_ids = {100}; + } + EXPECT_EQ(findNearestIndexFromLaneId(modified_path, create_point(8.5, -0.5, 0.0), 100), 8U); + EXPECT_EQ( + findNearestSegmentIndexFromLaneId(modified_path, create_point(8.5, -0.5, 0.0), 100), 8U); + } + + // Nearest is not within range + { + auto modified_path = path; + for (size_t i = 3; i < 9; ++i) { + modified_path.points.at(i).lane_ids = {100}; + } + EXPECT_EQ(findNearestIndexFromLaneId(modified_path, create_point(2.4, 1.3, 0.0), 100), 2U); + EXPECT_EQ( + findNearestSegmentIndexFromLaneId(modified_path, create_point(2.4, 1.3, 0.0), 100), 2U); + } + + // Path does not contain lane_id. + { + EXPECT_EQ(findNearestIndexFromLaneId(path, create_point(2.4, 1.3, 0.0), 100), 2U); + EXPECT_EQ(findNearestSegmentIndexFromLaneId(path, create_point(2.4, 1.3, 0.0), 100), 2U); + } + + // Empty points + EXPECT_THROW( + findNearestIndexFromLaneId(PathWithLaneId{}, create_point(2.4, 1.3, 0.0), 100), + std::invalid_argument); + EXPECT_THROW( + findNearestSegmentIndexFromLaneId(PathWithLaneId{}, create_point(2.4, 1.3, 0.0), 100), + std::invalid_argument); +} + +// NOTE: This test is temporary for the current implementation. +TEST(path_with_lane_id, convertToRearWheelCenter) +{ + using autoware::motion_utils::convertToRearWheelCenter; + + PathWithLaneId path; + + PathPointWithLaneId p1; + p1.point.pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + path.points.push_back(p1); + PathPointWithLaneId p2; + p2.point.pose = createPose(1.0, 2.0, 0.0, 0.0, 0.0, 0.0); + path.points.push_back(p2); + PathPointWithLaneId p3; + p3.point.pose = createPose(2.0, 4.0, 0.0, 0.0, 0.0, 0.0); + path.points.push_back(p3); + + const auto cog_path = convertToRearWheelCenter(path, 1.0); + + constexpr double epsilon = 1e-6; + EXPECT_NEAR(cog_path.points.at(0).point.pose.position.x, -0.4472136, epsilon); + EXPECT_NEAR(cog_path.points.at(0).point.pose.position.y, -0.8944272, epsilon); + EXPECT_NEAR(cog_path.points.at(1).point.pose.position.x, 0.5527864, epsilon); + EXPECT_NEAR(cog_path.points.at(1).point.pose.position.y, 1.1055728, epsilon); + EXPECT_NEAR(cog_path.points.at(2).point.pose.position.x, 2.0, epsilon); + EXPECT_NEAR(cog_path.points.at(2).point.pose.position.y, 4.0, epsilon); +} diff --git a/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp b/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp new file mode 100644 index 00000000..a2a93174 --- /dev/null +++ b/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp @@ -0,0 +1,5517 @@ +// 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/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware_utils/geometry/boost_geometry.hpp" +#include "autoware_utils/math/unit_conversion.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace +{ +using autoware_planning_msgs::msg::Trajectory; +using TrajectoryPointArray = std::vector; +using autoware_utils::create_point; +using autoware_utils::create_quaternion_from_rpy; +using autoware_utils::transform_point; + +constexpr double epsilon = 1e-6; + +geometry_msgs::msg::Pose createPose( + double x, double y, double z, double roll, double pitch, double yaw) +{ + geometry_msgs::msg::Pose p; + p.position = create_point(x, y, z); + p.orientation = create_quaternion_from_rpy(roll, pitch, yaw); + return p; +} + +template +T generateTestTrajectory( + const size_t num_points, const double point_interval, const double vel = 0.0, + const double init_theta = 0.0, const double delta_theta = 0.0) +{ + using Point = typename T::_points_type::value_type; + + T traj; + for (size_t i = 0; i < num_points; ++i) { + const double theta = init_theta + i * delta_theta; + const double x = i * point_interval * std::cos(theta); + const double y = i * point_interval * std::sin(theta); + + Point p; + p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); + p.longitudinal_velocity_mps = vel; + traj.points.push_back(p); + } + + return traj; +} + +TrajectoryPointArray generateTestTrajectoryPointArray( + const size_t num_points, const double point_interval, const double vel = 0.0, + const double init_theta = 0.0, const double delta_theta = 0.0) +{ + using autoware_planning_msgs::msg::TrajectoryPoint; + TrajectoryPointArray traj; + for (size_t i = 0; i < num_points; ++i) { + const double theta = init_theta + i * delta_theta; + const double x = i * point_interval * std::cos(theta); + const double y = i * point_interval * std::sin(theta); + + TrajectoryPoint p; + p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta); + p.longitudinal_velocity_mps = vel; + traj.push_back(p); + } + + return traj; +} + +template +void updateTrajectoryVelocityAt(T & points, const size_t idx, const double vel) +{ + points.at(idx).longitudinal_velocity_mps = vel; +} +} // namespace + +TEST(trajectory, validateNonEmpty) +{ + using autoware::motion_utils::validateNonEmpty; + + // Empty + EXPECT_THROW(validateNonEmpty(Trajectory{}.points), std::invalid_argument); + + // Non-empty + const auto traj = generateTestTrajectory(10, 1.0); + EXPECT_NO_THROW(validateNonEmpty(traj.points)); +} + +TEST(trajectory, validateNonSharpAngle_DefaultThreshold) +{ + using autoware::motion_utils::validateNonSharpAngle; + using autoware_planning_msgs::msg::TrajectoryPoint; + + TrajectoryPoint p1; + p1.pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + p1.longitudinal_velocity_mps = 0.0; + + TrajectoryPoint p2; + p2.pose = createPose(1.0, 1.0, 0.0, 0.0, 0.0, 0.0); + p2.longitudinal_velocity_mps = 0.0; + + TrajectoryPoint p3; + p3.pose = createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0); + p3.longitudinal_velocity_mps = 0.0; + + // Non sharp angle + { + EXPECT_NO_THROW(validateNonSharpAngle(p1, p2, p3)); + } + + // Sharp angle + { + EXPECT_THROW(validateNonSharpAngle(p2, p1, p3), std::invalid_argument); + EXPECT_THROW(validateNonSharpAngle(p1, p3, p2), std::invalid_argument); + } +} + +TEST(trajectory, validateNonSharpAngle_SetThreshold) +{ + using autoware::motion_utils::validateNonSharpAngle; + using autoware_planning_msgs::msg::TrajectoryPoint; + using autoware_utils::pi; + + TrajectoryPoint p1; + p1.pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + p1.longitudinal_velocity_mps = 0.0; + + TrajectoryPoint p2; + p2.pose = createPose(1.73205080756887729, 0.0, 0.0, 0.0, 0.0, 0.0); + p2.longitudinal_velocity_mps = 0.0; + + TrajectoryPoint p3; + p3.pose = createPose(1.73205080756887729, 1.0, 0.0, 0.0, 0.0, 0.0); + p3.longitudinal_velocity_mps = 0.0; + + // Non sharp angle + { + EXPECT_NO_THROW(validateNonSharpAngle(p1, p2, p3, pi / 6)); + EXPECT_NO_THROW(validateNonSharpAngle(p2, p3, p1, pi / 6)); + } + + // Sharp angle + { + EXPECT_THROW(validateNonSharpAngle(p3, p1, p2, pi / 6), std::invalid_argument); + } +} + +TEST(trajectory, searchZeroVelocityIndex) +{ + using autoware::motion_utils::searchZeroVelocityIndex; + + // Empty + EXPECT_FALSE(searchZeroVelocityIndex(Trajectory{}.points)); + + // No zero velocity point + { + const auto traj = generateTestTrajectory(10, 1.0, 1.0); + EXPECT_FALSE(searchZeroVelocityIndex(traj.points)); + } + + // Only start point is zero + { + const size_t idx_ans = 0; + + auto traj = generateTestTrajectory(10, 1.0, 1.0); + updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); + + EXPECT_EQ(*searchZeroVelocityIndex(traj.points), idx_ans); + } + + // Only end point is zero + { + const size_t idx_ans = 9; + + auto traj = generateTestTrajectory(10, 1.0, 1.0); + updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); + + EXPECT_EQ(*searchZeroVelocityIndex(traj.points), idx_ans); + } + + // Only middle point is zero + { + const size_t idx_ans = 5; + + auto traj = generateTestTrajectory(10, 1.0, 1.0); + updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); + + EXPECT_EQ(*searchZeroVelocityIndex(traj.points), idx_ans); + } + + // Two points are zero + { + const size_t idx_ans = 3; + + auto traj = generateTestTrajectory(10, 1.0, 1.0); + updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); + updateTrajectoryVelocityAt(traj.points, 6, 0.0); + + EXPECT_EQ(*searchZeroVelocityIndex(traj.points), idx_ans); + } + + // Negative velocity point is before zero velocity point + { + const size_t idx_ans = 3; + + auto traj = generateTestTrajectory(10, 1.0, 1.0); + updateTrajectoryVelocityAt(traj.points, 2, -1.0); + updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); + + EXPECT_EQ(*searchZeroVelocityIndex(traj.points), idx_ans); + } + + // Search from src_idx to dst_idx + { + const size_t idx_ans = 3; + + auto traj = generateTestTrajectory(10, 1.0, 1.0); + updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); + + EXPECT_FALSE(searchZeroVelocityIndex(traj.points, 0, 3)); + EXPECT_EQ(*searchZeroVelocityIndex(traj.points, 0, 4), idx_ans); + EXPECT_EQ(*searchZeroVelocityIndex(traj.points, 3, 10), idx_ans); + EXPECT_FALSE(searchZeroVelocityIndex(traj.points, 4, 10)); + } +} + +TEST(trajectory, searchZeroVelocityIndex_from_pose) +{ + using autoware::motion_utils::searchZeroVelocityIndex; + + // No zero velocity point + { + const auto traj = generateTestTrajectory(10, 1.0, 1.0); + EXPECT_FALSE(searchZeroVelocityIndex(traj.points, 0)); + } + + // Only start point is zero + { + const size_t idx_ans = 0; + + auto traj = generateTestTrajectory(10, 1.0, 1.0); + updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); + + EXPECT_EQ(*searchZeroVelocityIndex(traj.points, 0), idx_ans); + } + + // Only end point is zero + { + const size_t idx_ans = 9; + + auto traj = generateTestTrajectory(10, 1.0, 1.0); + updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); + + EXPECT_EQ(*searchZeroVelocityIndex(traj.points, 0), idx_ans); + } + + // Only middle point is zero + { + const size_t idx_ans = 5; + + auto traj = generateTestTrajectory(10, 1.0, 1.0); + updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); + + EXPECT_EQ(*searchZeroVelocityIndex(traj.points, 0), idx_ans); + } + + // Two points are zero + { + const size_t idx_ans = 3; + + auto traj = generateTestTrajectory(10, 1.0, 1.0); + updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); + updateTrajectoryVelocityAt(traj.points, 6, 0.0); + + EXPECT_EQ(*searchZeroVelocityIndex(traj.points, 0), idx_ans); + } + + // Negative velocity point is before zero velocity point + { + const size_t idx_ans = 3; + + auto traj = generateTestTrajectory(10, 1.0, 1.0); + updateTrajectoryVelocityAt(traj.points, 2, -1.0); + updateTrajectoryVelocityAt(traj.points, idx_ans, 0.0); + + EXPECT_EQ(*searchZeroVelocityIndex(traj.points, 0), idx_ans); + } +} + +TEST(trajectory, findNearestIndex_Pos_StraightTrajectory) +{ + using autoware::motion_utils::findNearestIndex; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Empty + EXPECT_THROW( + findNearestIndex(Trajectory{}.points, geometry_msgs::msg::Point{}), std::invalid_argument); + + // Start point + EXPECT_EQ(findNearestIndex(traj.points, create_point(0.0, 0.0, 0.0)), 0U); + + // End point + EXPECT_EQ(findNearestIndex(traj.points, create_point(9.0, 0.0, 0.0)), 9U); + + // Boundary conditions + EXPECT_EQ(findNearestIndex(traj.points, create_point(0.5, 0.0, 0.0)), 0U); + EXPECT_EQ(findNearestIndex(traj.points, create_point(0.51, 0.0, 0.0)), 1U); + + // Point before start point + EXPECT_EQ(findNearestIndex(traj.points, create_point(-4.0, 5.0, 0.0)), 0U); + + // Point after end point + EXPECT_EQ(findNearestIndex(traj.points, create_point(100.0, -3.0, 0.0)), 9U); + + // Random cases + EXPECT_EQ(findNearestIndex(traj.points, create_point(2.4, 1.3, 0.0)), 2U); + EXPECT_EQ(findNearestIndex(traj.points, create_point(4.0, 0.0, 0.0)), 4U); +} + +TEST(trajectory, findNearestIndex_Pos_CurvedTrajectory) +{ + using autoware::motion_utils::findNearestIndex; + + const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); + + // Random cases + EXPECT_EQ(findNearestIndex(traj.points, create_point(5.1, 3.4, 0.0)), 6U); +} + +TEST(trajectory, findNearestIndex_Pose_NoThreshold) +{ + using autoware::motion_utils::findNearestIndex; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Empty + EXPECT_FALSE(findNearestIndex(Trajectory{}.points, geometry_msgs::msg::Pose{}, {})); + + // Start point + EXPECT_EQ(*findNearestIndex(traj.points, createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)), 0U); + + // End point + EXPECT_EQ(*findNearestIndex(traj.points, createPose(9.0, 0.0, 0.0, 0.0, 0.0, 0.0)), 9U); + + // Boundary conditions + EXPECT_EQ(*findNearestIndex(traj.points, createPose(0.5, 0.0, 0.0, 0.0, 0.0, 0.0)), 0U); + EXPECT_EQ(*findNearestIndex(traj.points, createPose(0.51, 0.0, 0.0, 0.0, 0.0, 0.0)), 1U); + + // Point before start point + EXPECT_EQ(*findNearestIndex(traj.points, createPose(-4.0, 5.0, 0.0, 0.0, 0.0, 0.0)), 0U); + + // Point after end point + EXPECT_EQ(*findNearestIndex(traj.points, createPose(100.0, -3.0, 0.0, 0.0, 0.0, 0.0)), 9U); +} + +TEST(trajectory, findNearestIndex_Pose_DistThreshold) +{ + using autoware::motion_utils::findNearestIndex; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Out of threshold + EXPECT_FALSE(findNearestIndex(traj.points, createPose(3.0, 0.6, 0.0, 0.0, 0.0, 0.0), 0.5)); + + // On threshold + EXPECT_EQ(*findNearestIndex(traj.points, createPose(3.0, 0.5, 0.0, 0.0, 0.0, 0.0), 0.5), 3U); + + // Within threshold + EXPECT_EQ(*findNearestIndex(traj.points, createPose(3.0, 0.4, 0.0, 0.0, 0.0, 0.0), 0.5), 3U); +} + +TEST(trajectory, findNearestIndex_Pose_YawThreshold) +{ + using autoware::motion_utils::findNearestIndex; + + const auto traj = generateTestTrajectory(10, 1.0); + const auto max_d = std::numeric_limits::max(); + + // Out of threshold + EXPECT_FALSE(findNearestIndex(traj.points, createPose(3.0, 0.0, 0.0, 0.0, 0.0, 1.1), max_d, 1.0)); + + // On threshold + EXPECT_EQ( + *findNearestIndex(traj.points, createPose(3.0, 0.0, 0.0, 0.0, 0.0, 1.0), max_d, 1.0), 3U); + + // Within threshold + EXPECT_EQ( + *findNearestIndex(traj.points, createPose(3.0, 0.0, 0.0, 0.0, 0.0, 0.9), max_d, 1.0), 3U); +} + +TEST(trajectory, findNearestIndex_Pose_DistAndYawThreshold) +{ + using autoware::motion_utils::findNearestIndex; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Random cases + EXPECT_EQ(*findNearestIndex(traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0, 0.4), 2U); + EXPECT_EQ( + *findNearestIndex(traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8), 0.5, 1.0), 4U); + EXPECT_EQ( + *findNearestIndex(traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0), 1.0, 0.1), 8U); +} + +TEST(trajectory, findNearestSegmentIndex) +{ + using autoware::motion_utils::findNearestSegmentIndex; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Empty + EXPECT_THROW( + findNearestSegmentIndex(Trajectory{}.points, geometry_msgs::msg::Point{}), + std::invalid_argument); + + // Start point + EXPECT_EQ(findNearestSegmentIndex(traj.points, create_point(0.0, 0.0, 0.0)), 0U); + + // End point + EXPECT_EQ(findNearestSegmentIndex(traj.points, create_point(9.0, 0.0, 0.0)), 8U); + + // Boundary conditions + EXPECT_EQ(findNearestSegmentIndex(traj.points, create_point(1.0, 0.0, 0.0)), 0U); + EXPECT_EQ(findNearestSegmentIndex(traj.points, create_point(1.1, 0.0, 0.0)), 1U); + + // Point before start point + EXPECT_EQ(findNearestSegmentIndex(traj.points, create_point(-4.0, 5.0, 0.0)), 0U); + + // Point after end point + EXPECT_EQ(findNearestSegmentIndex(traj.points, create_point(100.0, -3.0, 0.0)), 8U); + + // Random cases + EXPECT_EQ(findNearestSegmentIndex(traj.points, create_point(2.4, 1.0, 0.0)), 2U); + EXPECT_EQ(findNearestSegmentIndex(traj.points, create_point(4.0, 0.0, 0.0)), 3U); + + // Two nearest trajectory points are not the edges of the nearest segment. + std::vector sparse_points{ + create_point(0.0, 0.0, 0.0), + create_point(10.0, 0.0, 0.0), + create_point(11.0, 0.0, 0.0), + }; + EXPECT_EQ(findNearestSegmentIndex(sparse_points, create_point(9.0, 1.0, 0.0)), 0U); +} + +TEST(trajectory, calcLongitudinalOffsetToSegment_StraightTrajectory) +{ + using autoware::motion_utils::calcLongitudinalOffsetToSegment; + + const auto traj = generateTestTrajectory(10, 1.0); + const bool throw_exception = true; + + // Empty + EXPECT_THROW( + calcLongitudinalOffsetToSegment( + Trajectory{}.points, {}, geometry_msgs::msg::Point{}, throw_exception), + std::invalid_argument); + + // Out of range + EXPECT_THROW( + calcLongitudinalOffsetToSegment(traj.points, -1, geometry_msgs::msg::Point{}, throw_exception), + std::out_of_range); + EXPECT_THROW( + calcLongitudinalOffsetToSegment( + traj.points, traj.points.size() - 1, geometry_msgs::msg::Point{}, throw_exception), + std::out_of_range); + + // Same close points in trajectory + { + const auto invalid_traj = generateTestTrajectory(10, 0.0); + const auto p = create_point(3.0, 0.0, 0.0); + EXPECT_THROW( + calcLongitudinalOffsetToSegment(invalid_traj.points, 3, p, throw_exception), + std::runtime_error); + } + + // Same point + EXPECT_NEAR( + calcLongitudinalOffsetToSegment(traj.points, 3, create_point(3.0, 0.0, 0.0)), 0.0, epsilon); + + // Point before start point + EXPECT_NEAR( + calcLongitudinalOffsetToSegment(traj.points, 6, create_point(-3.9, 3.0, 0.0)), -9.9, epsilon); + + // Point after start point + EXPECT_NEAR( + calcLongitudinalOffsetToSegment(traj.points, 7, create_point(13.3, -10.0, 0.0)), 6.3, epsilon); + + // Random cases + EXPECT_NEAR( + calcLongitudinalOffsetToSegment(traj.points, 2, create_point(4.3, 7.0, 0.0)), 2.3, epsilon); + EXPECT_NEAR( + calcLongitudinalOffsetToSegment(traj.points, 8, create_point(1.0, 3.0, 0.0)), -7, epsilon); +} + +TEST(trajectory, calcLongitudinalOffsetToSegment_CurveTrajectory) +{ + using autoware::motion_utils::calcLongitudinalOffsetToSegment; + + const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); + + // Random cases + EXPECT_NEAR( + calcLongitudinalOffsetToSegment(traj.points, 2, create_point(2.0, 0.5, 0.0)), 0.083861449, + epsilon); +} + +TEST(trajectory, calcLateralOffset) +{ + using autoware::motion_utils::calcLateralOffset; + + const auto traj = generateTestTrajectory(10, 1.0); + const bool throw_exception = true; + + // Empty + EXPECT_THROW( + calcLateralOffset(Trajectory{}.points, geometry_msgs::msg::Point{}, throw_exception), + std::invalid_argument); + + // Trajectory size is 1 + { + const auto one_point_traj = generateTestTrajectory(1, 1.0); + EXPECT_THROW( + calcLateralOffset(one_point_traj.points, geometry_msgs::msg::Point{}, throw_exception), + std::runtime_error); + } + + // Same close points in trajectory + { + const auto invalid_traj = generateTestTrajectory(10, 0.0); + const auto p = create_point(3.0, 0.0, 0.0); + EXPECT_THROW(calcLateralOffset(invalid_traj.points, p, throw_exception), std::runtime_error); + } + + // Point on trajectory + EXPECT_NEAR(calcLateralOffset(traj.points, create_point(3.1, 0.0, 0.0)), 0.0, epsilon); + + // Point before start point + EXPECT_NEAR(calcLateralOffset(traj.points, create_point(-3.9, 3.0, 0.0)), 3.0, epsilon); + + // Point after start point + EXPECT_NEAR(calcLateralOffset(traj.points, create_point(13.3, -10.0, 0.0)), -10.0, epsilon); + + // Random cases + EXPECT_NEAR(calcLateralOffset(traj.points, create_point(4.3, 7.0, 0.0)), 7.0, epsilon); + EXPECT_NEAR(calcLateralOffset(traj.points, create_point(1.0, -3.0, 0.0)), -3.0, epsilon); +} + +TEST(trajectory, calcLateralOffset_without_segment_idx) +{ + using autoware::motion_utils::calcLateralOffset; + + const auto traj = generateTestTrajectory(10, 1.0); + const bool throw_exception = true; + + // Empty + EXPECT_THROW( + calcLateralOffset(Trajectory{}.points, geometry_msgs::msg::Point{}, throw_exception), + std::invalid_argument); + + // Trajectory size is 1 + { + const auto one_point_traj = generateTestTrajectory(1, 1.0); + EXPECT_THROW( + calcLateralOffset( + one_point_traj.points, geometry_msgs::msg::Point{}, static_cast(0), + throw_exception), + std::runtime_error); + } + + // Same close points in trajectory + { + const auto invalid_traj = generateTestTrajectory(10, 0.0); + const auto p = create_point(3.0, 0.0, 0.0); + EXPECT_THROW( + calcLateralOffset(invalid_traj.points, p, static_cast(2), throw_exception), + std::runtime_error); + EXPECT_THROW( + calcLateralOffset(invalid_traj.points, p, static_cast(3), throw_exception), + std::runtime_error); + } + + // Point on trajectory + EXPECT_NEAR( + calcLateralOffset(traj.points, create_point(3.1, 0.0, 0.0), static_cast(3)), 0.0, + epsilon); + + // Point before start point + EXPECT_NEAR( + calcLateralOffset(traj.points, create_point(-3.9, 3.0, 0.0), static_cast(0)), 3.0, + epsilon); + + // Point after start point + EXPECT_NEAR( + calcLateralOffset(traj.points, create_point(13.3, -10.0, 0.0), static_cast(8)), -10.0, + epsilon); + + // Random cases + EXPECT_NEAR( + calcLateralOffset(traj.points, create_point(4.3, 7.0, 0.0), static_cast(4)), 7.0, + epsilon); + EXPECT_NEAR( + calcLateralOffset(traj.points, create_point(1.0, -3.0, 0.0), static_cast(0)), -3.0, + epsilon); + EXPECT_NEAR( + calcLateralOffset(traj.points, create_point(1.0, -3.0, 0.0), static_cast(1)), -3.0, + epsilon); +} + +TEST(trajectory, calcLateralOffset_CurveTrajectory) +{ + using autoware::motion_utils::calcLateralOffset; + + const auto traj = generateTestTrajectory(10, 1.0, 0.0, 0.0, 0.1); + + // Random cases + EXPECT_NEAR(calcLateralOffset(traj.points, create_point(2.0, 0.5, 0.0)), 0.071386083, epsilon); + EXPECT_NEAR(calcLateralOffset(traj.points, create_point(5.0, 1.0, 0.0)), -1.366602819, epsilon); +} + +TEST(trajectory, calcSignedArcLengthFromIndexToIndex) +{ + using autoware::motion_utils::calcSignedArcLength; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Empty + EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); + + // Out of range + EXPECT_THROW(calcSignedArcLength(traj.points, -1, 1), std::out_of_range); + EXPECT_THROW(calcSignedArcLength(traj.points, 0, traj.points.size() + 1), std::out_of_range); + + // Same point + EXPECT_NEAR(calcSignedArcLength(traj.points, 3, 3), 0, epsilon); + + // Forward + EXPECT_NEAR(calcSignedArcLength(traj.points, 0, 3), 3, epsilon); + + // Backward + EXPECT_NEAR(calcSignedArcLength(traj.points, 9, 5), -4, epsilon); +} + +TEST(trajectory, calcSignedArcLengthFromPointToIndex) +{ + using autoware::motion_utils::calcSignedArcLength; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Empty + EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); + + // Same point + EXPECT_NEAR(calcSignedArcLength(traj.points, create_point(3.0, 0.0, 0.0), 3), 0, epsilon); + + // Forward + EXPECT_NEAR(calcSignedArcLength(traj.points, create_point(0.0, 0.0, 0.0), 3), 3, epsilon); + + // Backward + EXPECT_NEAR(calcSignedArcLength(traj.points, create_point(9.0, 0.0, 0.0), 5), -4, epsilon); + + // Point before start point + EXPECT_NEAR(calcSignedArcLength(traj.points, create_point(-3.9, 3.0, 0.0), 6), 9.9, epsilon); + + // Point after end point + EXPECT_NEAR(calcSignedArcLength(traj.points, create_point(13.3, -10.0, 0.0), 7), -6.3, epsilon); + + // Random cases + EXPECT_NEAR(calcSignedArcLength(traj.points, create_point(1.0, 3.0, 0.0), 9), 8, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, create_point(4.3, 7.0, 0.0), 2), -2.3, epsilon); +} + +TEST(trajectory, calcSignedArcLengthFromIndexToPoint) +{ + using autoware::motion_utils::calcSignedArcLength; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Empty + EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); + + // Same point + EXPECT_NEAR(calcSignedArcLength(traj.points, 3, create_point(3.0, 0.0, 0.0)), 0, epsilon); + + // Forward + EXPECT_NEAR(calcSignedArcLength(traj.points, 0, create_point(3.0, 0.0, 0.0)), 3, epsilon); + + // Backward + EXPECT_NEAR(calcSignedArcLength(traj.points, 9, create_point(5.0, 0.0, 0.0)), -4, epsilon); + + // Point before start point + EXPECT_NEAR(calcSignedArcLength(traj.points, 6, create_point(-3.9, 3.0, 0.0)), -9.9, epsilon); + + // Point after end point + EXPECT_NEAR(calcSignedArcLength(traj.points, 7, create_point(13.3, -10.0, 0.0)), 6.3, epsilon); + + // Random cases + EXPECT_NEAR(calcSignedArcLength(traj.points, 1, create_point(9.0, 3.0, 0.0)), 8, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, 4, create_point(1.7, 7.0, 0.0)), -2.3, epsilon); +} + +TEST(trajectory, calcSignedArcLengthFromPointToPoint) +{ + using autoware::motion_utils::calcSignedArcLength; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Empty + EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); + + // Same point + { + const auto p = create_point(3.0, 0.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p, p), 0, epsilon); + } + + // Forward + { + const auto p1 = create_point(0.0, 0.0, 0.0); + const auto p2 = create_point(3.0, 1.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), 3, epsilon); + } + + // Backward + { + const auto p1 = create_point(8.0, 0.0, 0.0); + const auto p2 = create_point(9.0, 0.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), 1, epsilon); + } + + // Point before start point + { + const auto p1 = create_point(-3.9, 3.0, 0.0); + const auto p2 = create_point(6.0, -10.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), 9.9, epsilon); + } + + // Point after end point + { + const auto p1 = create_point(7.0, -5.0, 0.0); + const auto p2 = create_point(13.3, -10.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), 6.3, epsilon); + } + + // Point before start point and after end point + { + const auto p1 = create_point(-4.3, 10.0, 0.0); + const auto p2 = create_point(13.8, -1.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), 18.1, epsilon); + } + + // Random cases + { + const auto p1 = create_point(1.0, 3.0, 0.0); + const auto p2 = create_point(9.0, -1.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), 8, epsilon); + } + { + const auto p1 = create_point(4.3, 7.0, 0.0); + const auto p2 = create_point(2.0, 3.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, p2), -2.3, epsilon); + } +} + +TEST(trajectory, calcArcLength) +{ + using autoware::motion_utils::calcArcLength; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Empty + EXPECT_DOUBLE_EQ(calcArcLength(Trajectory{}.points), 0.0); + + // Whole Length + EXPECT_NEAR(calcArcLength(traj.points), 9.0, epsilon); +} + +TEST(trajectory, convertToTrajectory) +{ + using autoware::motion_utils::convertToTrajectory; + + // Size check + { + const auto traj_input = generateTestTrajectoryPointArray(50, 1.0); + const auto traj = convertToTrajectory(traj_input); + EXPECT_EQ(traj.points.size(), traj_input.size()); + } +} + +TEST(trajectory, convertToTrajectoryPointArray) +{ + using autoware::motion_utils::convertToTrajectoryPointArray; + + const auto traj_input = generateTestTrajectory(100, 1.0); + const auto traj = convertToTrajectoryPointArray(traj_input); + + // Size check + EXPECT_EQ(traj.size(), traj_input.points.size()); + + // Value check + for (size_t i = 0; i < traj.size(); ++i) { + EXPECT_EQ(traj.at(i), traj_input.points.at(i)); + } +} + +TEST(trajectory, calcDistanceToForwardStopPointFromIndex) +{ + using autoware::motion_utils::calcDistanceToForwardStopPoint; + + auto traj_input = generateTestTrajectory(100, 1.0, 3.0); + traj_input.points.at(50).longitudinal_velocity_mps = 0.0; + + // Empty + { + EXPECT_FALSE(calcDistanceToForwardStopPoint(Trajectory{}.points)); + } + + // No Stop Point + { + const auto traj_no_stop_point = generateTestTrajectory(10, 1.0, 3.0); + for (size_t i = 0; i < 10; ++i) { + const auto dist = calcDistanceToForwardStopPoint(traj_no_stop_point.points, i); + EXPECT_FALSE(dist); + } + } + + // Boundary1 (Edge of the input trajectory) + { + const auto dist = calcDistanceToForwardStopPoint(traj_input.points, 0); + EXPECT_NEAR(dist.value(), 50.0, epsilon); + } + + // Boundary2 (Edge of the input trajectory) + { + const auto dist = calcDistanceToForwardStopPoint(traj_input.points, 99); + EXPECT_FALSE(dist); + } + + // Boundary3 (On the Stop Point) + { + const auto dist = calcDistanceToForwardStopPoint(traj_input.points, 50); + EXPECT_NEAR(dist.value(), 0.0, epsilon); + } + + // Boundary4 (Right before the stop point) + { + const auto dist = calcDistanceToForwardStopPoint(traj_input.points, 49); + EXPECT_NEAR(dist.value(), 1.0, epsilon); + } + + // Boundary5 (Right behind the stop point) + { + const auto dist = calcDistanceToForwardStopPoint(traj_input.points, 51); + EXPECT_FALSE(dist); + } + + // Random + { + const auto dist = calcDistanceToForwardStopPoint(traj_input.points, 20); + EXPECT_NEAR(dist.value(), 30.0, epsilon); + } +} + +TEST(trajectory, calcDistanceToForwardStopPointFromPose) +{ + using autoware::motion_utils::calcDistanceToForwardStopPoint; + + auto traj_input = generateTestTrajectory(100, 1.0, 3.0); + traj_input.points.at(50).longitudinal_velocity_mps = 0.0; + + // Empty + { + EXPECT_FALSE(calcDistanceToForwardStopPoint(Trajectory{}.points, geometry_msgs::msg::Pose{})); + } + + // No Stop Point + { + const auto traj_no_stop_point = generateTestTrajectory(100, 1.0, 3.0); + const auto pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto dist = calcDistanceToForwardStopPoint(traj_no_stop_point.points, pose); + EXPECT_FALSE(dist); + } + + // Trajectory Edge1 + { + const auto pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); + EXPECT_NEAR(dist.value(), 50.0, epsilon); + } + + // Trajectory Edge2 + { + const auto pose = createPose(99.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); + EXPECT_FALSE(dist); + } + + // Out of Trajectory1 + { + const auto pose = createPose(-10.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); + EXPECT_NEAR(dist.value(), 60.0, epsilon); + } + + // Out of Trajectory2 + { + const auto pose = createPose(200.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); + EXPECT_FALSE(dist); + } + + // Out of Trajectory3 + { + const auto pose = createPose(-30.0, 50.0, 0.0, 0.0, 0.0, 0.0); + const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); + EXPECT_NEAR(dist.value(), 80.0, epsilon); + } + + // Boundary Condition1 + { + const auto pose = createPose(50.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); + EXPECT_NEAR(dist.value(), 0.0, epsilon); + } + + // Boundary Condition2 + { + const auto pose = createPose(50.1, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); + EXPECT_FALSE(dist); + } + + // Boundary Condition3 + { + const auto pose = createPose(49.9, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); + EXPECT_NEAR(dist.value(), 0.1, epsilon); + } + + // Random + { + const auto pose = createPose(3.0, 2.0, 0.0, 0.0, 0.0, 0.0); + const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); + EXPECT_NEAR(dist.value(), 47.0, epsilon); + } +} + +TEST(trajectory, calcDistanceToForwardStopPoint_DistThreshold) +{ + using autoware::motion_utils::calcDistanceToForwardStopPoint; + + auto traj_input = generateTestTrajectory(100, 1.0, 3.0); + traj_input.points.at(50).longitudinal_velocity_mps = 0.0; + + // Boundary Condition1 + { + const auto pose = createPose(-4.9, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, 5.0); + EXPECT_NEAR(dist.value(), 54.9, epsilon); + } + + // Boundary Condition2 + { + const auto pose = createPose(-5.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, 5.0); + EXPECT_NEAR(dist.value(), 55.0, epsilon); + } + + // Boundary Condition3 + { + const auto pose = createPose(-5.1, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, 5.0); + EXPECT_FALSE(dist); + } + + // Random + { + { + const auto pose = createPose(3.0, 2.0, 0.0, 0.0, 0.0, 0.0); + const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, 5.0); + EXPECT_NEAR(dist.value(), 47.0, epsilon); + } + + { + const auto pose = createPose(200.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, 5.0); + EXPECT_FALSE(dist); + } + } +} + +TEST(trajectory, calcDistanceToForwardStopPoint_YawThreshold) +{ + using autoware::motion_utils::calcDistanceToForwardStopPoint; + using autoware_utils::deg2rad; + + const auto max_d = std::numeric_limits::max(); + auto traj_input = generateTestTrajectory(100, 1.0, 3.0); + traj_input.points.at(50).longitudinal_velocity_mps = 0.0; + + // Boundary Condition + { + const double x = 2.0; + { + const auto pose = createPose(x, 0.0, 0.0, 0.0, 0.0, deg2rad(29.9)); + const auto dist = + calcDistanceToForwardStopPoint(traj_input.points, pose, max_d, deg2rad(30.0)); + EXPECT_NEAR(dist.value(), 48.0, epsilon); + } + + { + const auto pose = createPose(x, 0.0, 0.0, 0.0, 0.0, deg2rad(30.0)); + const auto dist = + calcDistanceToForwardStopPoint(traj_input.points, pose, max_d, deg2rad(30.0)); + EXPECT_NEAR(dist.value(), 48.0, epsilon); + } + + { + const auto pose = createPose(x, 0.0, 0.0, 0.0, 0.0, deg2rad(30.1)); + const auto dist = + calcDistanceToForwardStopPoint(traj_input.points, pose, max_d, deg2rad(30.0)); + EXPECT_FALSE(dist); + } + } + + // Random + { + { + const auto pose = createPose(3.0, 2.0, 0.0, 0.0, 0.0, deg2rad(15.0)); + const auto dist = + calcDistanceToForwardStopPoint(traj_input.points, pose, max_d, deg2rad(20.0)); + EXPECT_NEAR(dist.value(), 47.0, epsilon); + } + + { + const auto pose = createPose(15.0, 30.0, 0.0, 0.0, 0.0, deg2rad(45.0)); + const auto dist = + calcDistanceToForwardStopPoint(traj_input.points, pose, max_d, deg2rad(10.0)); + EXPECT_FALSE(dist); + } + } +} + +TEST(trajectory, calcLongitudinalOffsetPointFromIndex) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPoint; + using autoware::motion_utils::calcSignedArcLength; + using autoware_utils::get_point; + + const auto traj = generateTestTrajectory(10, 1.0); + const auto total_length = calcArcLength(traj.points); + + // Empty + EXPECT_FALSE(calcLongitudinalOffsetPoint(Trajectory{}.points, {}, {})); + + // Out of range + EXPECT_FALSE(calcLongitudinalOffsetPoint(traj.points, traj.points.size() + 1, 1.0)); + EXPECT_FALSE(calcLongitudinalOffsetPoint(traj.points, -1, 1.0)); + + // Found Pose(forward) + for (size_t i = 0; i < traj.points.size(); ++i) { + double x_ans = get_point(traj.points.at(i)).x; + + const auto d_back = calcSignedArcLength(traj.points, i, traj.points.size() - 1); + + for (double len = 0.0; len < d_back + epsilon; len += 0.1) { + const auto p_out = calcLongitudinalOffsetPoint(traj.points, i, std::min(len, d_back)); + + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().z, 0.0, epsilon); + + x_ans += 0.1; + } + } + + // Found Pose(backward) + for (size_t i = 0; i < traj.points.size(); ++i) { + double x_ans = get_point(traj.points.at(i)).x; + + const auto d_front = calcSignedArcLength(traj.points, i, 0); + + for (double len = 0.0; d_front - epsilon < len; len -= 0.1) { + const auto p_out = calcLongitudinalOffsetPoint(traj.points, i, std::max(len, d_front)); + + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().z, 0.0, epsilon); + + x_ans -= 0.1; + } + } + + // No found + { + const auto p_out = calcLongitudinalOffsetPoint(traj.points, 0, total_length + epsilon); + + EXPECT_EQ(p_out, std::nullopt); + } + + // No found + { + const auto p_out = calcLongitudinalOffsetPoint(traj.points, 9, -total_length - epsilon); + + EXPECT_EQ(p_out, std::nullopt); + } + + // No found(Trajectory size is 1) + { + const auto one_point_traj = generateTestTrajectory(1, 1.0); + const auto p_out = calcLongitudinalOffsetPoint(one_point_traj.points, 0.0, 0.0); + + EXPECT_EQ(p_out, std::nullopt); + } +} + +TEST(trajectory, calcLongitudinalOffsetPointFromPoint) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPoint; + using autoware::motion_utils::calcSignedArcLength; + using autoware_utils::create_point; + using autoware_utils::get_point; + + const auto traj = generateTestTrajectory(10, 1.0); + const auto total_length = calcArcLength(traj.points); + + // Empty + EXPECT_FALSE(calcLongitudinalOffsetPoint(Trajectory{}.points, {}, {})); + + // Found Pose(forward) + for (double x_start = 0.0; x_start < total_length + epsilon; x_start += 0.1) { + constexpr double lateral_deviation = 0.5; + double x_ans = x_start; + + const auto p_src = create_point(x_start, lateral_deviation, 0.0); + const auto d_back = calcSignedArcLength(traj.points, p_src, traj.points.size() - 1); + + for (double len = 0.0; len < d_back + epsilon; len += 0.1) { + const auto p_out = calcLongitudinalOffsetPoint(traj.points, p_src, std::min(len, d_back)); + + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().z, 0.0, epsilon); + + x_ans += 0.1; + } + } + + // Found Pose(backward) + for (double x_start = 0.0; x_start < total_length + epsilon; x_start += 0.1) { + constexpr double lateral_deviation = 0.5; + double x_ans = x_start; + + const auto p_src = create_point(x_start, lateral_deviation, 0.0); + const auto d_front = calcSignedArcLength(traj.points, p_src, 0); + + for (double len = 0.0; d_front - epsilon < len; len -= 0.1) { + const auto p_out = calcLongitudinalOffsetPoint(traj.points, p_src, std::max(len, d_front)); + + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().z, 0.0, epsilon); + + x_ans -= 0.1; + } + } + + // No found + { + const auto p_src = create_point(0.0, 0.0, 0.0); + const auto p_out = calcLongitudinalOffsetPoint(traj.points, p_src, total_length + 1.0); + + EXPECT_EQ(p_out, std::nullopt); + } + + // No found + { + const auto p_src = create_point(9.0, 0.0, 0.0); + const auto p_out = calcLongitudinalOffsetPoint(traj.points, p_src, -total_length - 1.0); + + EXPECT_EQ(p_out, std::nullopt); + } + + // Out of range(Trajectory size is 1) + { + const auto one_point_traj = generateTestTrajectory(1, 1.0); + EXPECT_FALSE( + calcLongitudinalOffsetPoint(one_point_traj.points, geometry_msgs::msg::Point{}, {})); + } +} + +TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPose; + using autoware::motion_utils::calcSignedArcLength; + using autoware_utils::get_point; + + const auto traj = generateTestTrajectory(10, 1.0); + const auto total_length = calcArcLength(traj.points); + + // Empty + EXPECT_FALSE(calcLongitudinalOffsetPose(Trajectory{}.points, {}, {})); + + // Out of range + EXPECT_FALSE(calcLongitudinalOffsetPose(traj.points, traj.points.size() + 1, 1.0)); + EXPECT_FALSE(calcLongitudinalOffsetPose(traj.points, -1, 1.0)); + + // Found Pose(forward) + for (size_t i = 0; i < traj.points.size(); ++i) { + double x_ans = get_point(traj.points.at(i)).x; + + const auto d_back = calcSignedArcLength(traj.points, i, traj.points.size() - 1); + + for (double len = 0.0; len < d_back + epsilon; len += 0.1) { + const auto p_out = calcLongitudinalOffsetPose(traj.points, i, std::min(len, d_back)); + + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().position.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, 1.0, epsilon); + + x_ans += 0.1; + } + } + + // Found Pose(backward) + for (size_t i = 0; i < traj.points.size(); ++i) { + double x_ans = get_point(traj.points.at(i)).x; + + const auto d_front = calcSignedArcLength(traj.points, i, 0); + + for (double len = 0.0; d_front - epsilon < len; len -= 0.1) { + const auto p_out = calcLongitudinalOffsetPose(traj.points, i, std::max(len, d_front)); + + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().position.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, 1.0, epsilon); + + x_ans -= 0.1; + } + } + + // No found + { + const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, total_length + epsilon); + + EXPECT_EQ(p_out, std::nullopt); + } + + // No found + { + const auto p_out = calcLongitudinalOffsetPose(traj.points, 9, -total_length - epsilon); + + EXPECT_EQ(p_out, std::nullopt); + } + + // No found(Trajectory size is 1) + { + const auto one_point_traj = generateTestTrajectory(1, 1.0); + const auto p_out = calcLongitudinalOffsetPose(one_point_traj.points, 0.0, 0.0); + + EXPECT_EQ(p_out, std::nullopt); + } +} + +TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPose; + using autoware_planning_msgs::msg::TrajectoryPoint; + using autoware_utils::deg2rad; + + Trajectory traj{}; + + { + TrajectoryPoint p; + p.pose = createPose(0.0, 0.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); + p.longitudinal_velocity_mps = 0.0; + traj.points.push_back(p); + } + + { + TrajectoryPoint p; + p.pose = createPose(1.0, 1.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + p.longitudinal_velocity_mps = 0.0; + traj.points.push_back(p); + } + + const auto total_length = calcArcLength(traj.points); + + // Found pose(forward) + for (double len = 0.0; len < total_length; len += 0.1) { + const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, len); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); + + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, len * std::cos(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.y, len * std::sin(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); + } + + // Found pose(backward) + for (double len = total_length; 0.0 < len; len -= 0.1) { + const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, -len); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); + + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0 - len * std::cos(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0 - len * std::sin(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); + } + + // Boundary condition + { + const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, total_length); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); + } + + // Boundary condition + { + const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, 0.0); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); + } +} + +TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPose; + using autoware_planning_msgs::msg::TrajectoryPoint; + using autoware_utils::deg2rad; + + Trajectory traj{}; + + { + TrajectoryPoint p; + p.pose = createPose(0.0, 0.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); + p.longitudinal_velocity_mps = 0.0; + traj.points.push_back(p); + } + + { + TrajectoryPoint p; + p.pose = createPose(1.0, 1.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + p.longitudinal_velocity_mps = 0.0; + traj.points.push_back(p); + } + + const auto total_length = calcArcLength(traj.points); + + // Found pose(forward) + for (double len = 0.0; len < total_length; len += 0.1) { + const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, len, false); + // ratio between two points + const auto ratio = len / total_length; + const auto ans_quat = + create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 - 45.0 * ratio)); + + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, len * std::cos(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.y, len * std::sin(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); + } + + // Found pose(backward) + for (double len = total_length; 0.0 < len; len -= 0.1) { + const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, -len, false); + // ratio between two points + const auto ratio = len / total_length; + const auto ans_quat = + create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 * ratio)); + + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0 - len * std::cos(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0 - len * std::sin(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); + } + + // Boundary condition + { + const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, total_length, false); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); + } + + // Boundary condition + { + const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, 0.0, false); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); + } +} + +TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPose; + using autoware::motion_utils::calcSignedArcLength; + using autoware_utils::create_point; + using autoware_utils::get_point; + + const auto traj = generateTestTrajectory(10, 1.0); + const auto total_length = calcArcLength(traj.points); + + // Empty + EXPECT_FALSE(calcLongitudinalOffsetPose(Trajectory{}.points, {}, {})); + + // Found Pose(forward) + for (double x_start = 0.0; x_start < total_length + epsilon; x_start += 0.1) { + constexpr double lateral_deviation = 0.5; + double x_ans = x_start; + + const auto p_src = create_point(x_start, lateral_deviation, 0.0); + const auto d_back = calcSignedArcLength(traj.points, p_src, traj.points.size() - 1); + + for (double len = 0.0; len < d_back + epsilon; len += 0.1) { + const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, std::min(len, d_back)); + + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().position.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, 1.0, epsilon); + + x_ans += 0.1; + } + } + + // Found Pose(backward) + for (double x_start = 0.0; x_start < total_length + epsilon; x_start += 0.1) { + constexpr double lateral_deviation = 0.5; + double x_ans = x_start; + + const auto p_src = create_point(x_start, lateral_deviation, 0.0); + const auto d_front = calcSignedArcLength(traj.points, p_src, 0); + + for (double len = 0.0; d_front - epsilon < len; len -= 0.1) { + const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, std::max(len, d_front)); + + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().position.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, 1.0, epsilon); + + x_ans -= 0.1; + } + } + + // No found + { + const auto p_src = create_point(0.0, 0.0, 0.0); + const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, total_length + 1.0); + + EXPECT_EQ(p_out, std::nullopt); + } + + // No found + { + const auto p_src = create_point(9.0, 0.0, 0.0); + const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, -total_length - 1.0); + + EXPECT_EQ(p_out, std::nullopt); + } + + // Out of range(Trajectory size is 1) + { + const auto one_point_traj = generateTestTrajectory(1, 1.0); + EXPECT_FALSE( + calcLongitudinalOffsetPose(one_point_traj.points, geometry_msgs::msg::Point{}, {})); + } +} + +TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPose; + using autoware::motion_utils::calcLongitudinalOffsetToSegment; + using autoware_planning_msgs::msg::TrajectoryPoint; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + + Trajectory traj{}; + + { + TrajectoryPoint p; + p.pose = createPose(0.0, 0.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); + p.longitudinal_velocity_mps = 0.0; + traj.points.push_back(p); + } + + { + TrajectoryPoint p; + p.pose = createPose(1.0, 1.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + p.longitudinal_velocity_mps = 0.0; + traj.points.push_back(p); + } + + const auto total_length = calcArcLength(traj.points); + + // Found pose + for (double len_start = 0.0; len_start < total_length; len_start += 0.1) { + constexpr double deviation = 0.1; + + const auto p_src = create_point( + len_start * std::cos(deg2rad(45.0)) + deviation, + len_start * std::sin(deg2rad(45.0)) - deviation, 0.0); + const auto src_offset = calcLongitudinalOffsetToSegment(traj.points, 0, p_src); + + for (double len = -src_offset; len < total_length - src_offset; len += 0.1) { + const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, len); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); + + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR( + p_out.value().position.x, p_src.x + len * std::cos(deg2rad(45.0)) - deviation, epsilon); + EXPECT_NEAR( + p_out.value().position.y, p_src.y + len * std::sin(deg2rad(45.0)) + deviation, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary condition + { + constexpr double deviation = 0.1; + + const auto p_src = create_point(1.0 + deviation, 1.0 - deviation, 0.0); + const auto src_offset = calcLongitudinalOffsetToSegment(traj.points, 0, p_src); + + const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, total_length - src_offset); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); + } +} + +TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::calcLongitudinalOffsetPose; + using autoware::motion_utils::calcLongitudinalOffsetToSegment; + using autoware_planning_msgs::msg::TrajectoryPoint; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + + Trajectory traj{}; + + { + TrajectoryPoint p; + p.pose = createPose(0.0, 0.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); + p.longitudinal_velocity_mps = 0.0; + traj.points.push_back(p); + } + + { + TrajectoryPoint p; + p.pose = createPose(1.0, 1.0, 0.0, deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + p.longitudinal_velocity_mps = 0.0; + traj.points.push_back(p); + } + + const auto total_length = calcArcLength(traj.points); + + // Found pose + for (double len_start = 0.0; len_start < total_length; len_start += 0.1) { + constexpr double deviation = 0.1; + + const auto p_src = create_point( + len_start * std::cos(deg2rad(45.0)) + deviation, + len_start * std::sin(deg2rad(45.0)) - deviation, 0.0); + const auto src_offset = calcLongitudinalOffsetToSegment(traj.points, 0, p_src); + + for (double len = -src_offset; len < total_length - src_offset; len += 0.1) { + const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, len, false); + // ratio between two points + const auto ratio = (src_offset + len) / total_length; + const auto ans_quat = + create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 - 45.0 * ratio)); + + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR( + p_out.value().position.x, p_src.x + len * std::cos(deg2rad(45.0)) - deviation, epsilon); + EXPECT_NEAR( + p_out.value().position.y, p_src.y + len * std::sin(deg2rad(45.0)) + deviation, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary condition + { + constexpr double deviation = 0.1; + + const auto p_src = create_point(1.0 + deviation, 1.0 - deviation, 0.0); + const auto src_offset = calcLongitudinalOffsetToSegment(traj.points, 0, p_src); + + const auto p_out = + calcLongitudinalOffsetPose(traj.points, p_src, total_length - src_offset, false); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); + } +} + +TEST(trajectory, insertTargetPoint) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertTargetPoint; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; + + const auto traj = generateTestTrajectory(10, 1.0); + const auto total_length = calcArcLength(traj.points); + + // Insert + for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Insert(Boundary condition) + for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start + 1.1e-3, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Insert(Quaternion interpolation) + for (double x_start = 0.25; x_start < total_length; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start, 0.25 * std::tan(deg2rad(60.0)), 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(60.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Not insert(Overlap base_idx point) + for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start + 1e-4, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + } + + // Not insert(Overlap base_idx + 1 point) + for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start - 1e-4, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + } + + // Invalid target point(In front of begin point) + { + auto traj_out = traj; + + const auto p_target = create_point(-1.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); + + EXPECT_EQ(insert_idx, std::nullopt); + } + + // Invalid target point(Behind of end point) + { + auto traj_out = traj; + + const auto p_target = create_point(10.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); + + EXPECT_EQ(insert_idx, std::nullopt); + } + + // Invalid target point(Huge lateral offset) + { + auto traj_out = traj; + + const auto p_target = create_point(4.0, 10.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); + + EXPECT_EQ(insert_idx, std::nullopt); + } + + // Invalid base index + { + auto traj_out = traj; + + const size_t segment_idx = 9U; + const auto p_target = create_point(10.0, 0.0, 0.0); + const auto insert_idx = insertTargetPoint(segment_idx, p_target, traj_out.points); + + EXPECT_EQ(insert_idx, std::nullopt); + } + + // Empty + { + auto empty_traj = generateTestTrajectory(0, 1.0); + const size_t segment_idx = 0; + EXPECT_FALSE(insertTargetPoint(segment_idx, geometry_msgs::msg::Point{}, empty_traj.points)); + } +} + +TEST(trajectory, insertTargetPoint_Reverse) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertTargetPoint; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::create_quaternion_from_yaw; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; + + constexpr double overlap_threshold = 1e-4; + auto traj = generateTestTrajectory(10, 1.0); + for (size_t i = 0; i < traj.points.size(); ++i) { + traj.points.at(i).pose.orientation = create_quaternion_from_yaw(autoware_utils::pi); + } + const auto total_length = calcArcLength(traj.points); + + for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start + 1.1e-4, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = + insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE( + calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(180)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(180)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } +} + +TEST(trajectory, insertTargetPoint_OverlapThreshold) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertTargetPoint; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; + + constexpr double overlap_threshold = 1e-4; + const auto traj = generateTestTrajectory(10, 1.0); + const auto total_length = calcArcLength(traj.points); + + // Insert(Boundary condition) + for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start + 1.1e-4, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = + insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE( + calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Not insert(Overlap base_idx point) + for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start + 1e-5, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = + insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE( + calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold); + } + } + + // Not insert(Overlap base_idx + 1 point) + for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start - 1e-5, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = + insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE( + calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > overlap_threshold); + } + } +} + +TEST(trajectory, insertTargetPoint_Length) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertTargetPoint; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; + + const auto traj = generateTestTrajectory(10, 1.0); + const auto total_length = calcArcLength(traj.points); + + // Insert + for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(x_start, p_target, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Insert(Boundary condition) + for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start + 1.1e-3, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(x_start + 1.1e-3, p_target, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Right on the terminal point + { + auto traj_out = traj; + + const auto p_target = create_point(9.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(9.0, p_target, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Insert(Quaternion interpolation) + for (double x_start = 0.25; x_start < total_length; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start, 0.25 * std::tan(deg2rad(60.0)), 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(x_start, p_target, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(60.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Not insert(Overlap base_idx point) + for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start + 1e-4, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(x_start + 1e-4, p_target, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + } + + // Not insert(Overlap base_idx + 1 point) + for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start - 1e-4, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(x_start - 1e-4, p_target, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + } + + // Invalid target point(In front of the beginning point) + { + auto traj_out = traj; + + const auto p_target = create_point(-1.0, 0.0, 0.0); + const auto insert_idx = insertTargetPoint(-1.0, p_target, traj_out.points); + + EXPECT_EQ(insert_idx, std::nullopt); + } + + // Invalid target point(Behind the end point) + { + auto traj_out = traj; + + const auto p_target = create_point(10.0, 0.0, 0.0); + const auto insert_idx = insertTargetPoint(10.0, p_target, traj_out.points); + + EXPECT_EQ(insert_idx, std::nullopt); + } + + // Invalid target point(Huge lateral offset) + { + auto traj_out = traj; + + const auto p_target = create_point(4.0, 10.0, 0.0); + const auto insert_idx = insertTargetPoint(4.0, p_target, traj_out.points); + + EXPECT_EQ(insert_idx, std::nullopt); + } + + // Empty + { + auto empty_traj = generateTestTrajectory(0, 1.0); + EXPECT_THROW( + insertTargetPoint(0.0, geometry_msgs::msg::Point{}, empty_traj.points), + std::invalid_argument); + } +} + +TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertTargetPoint; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; + + const auto traj = generateTestTrajectory(10, 1.0); + const auto total_length = calcArcLength(traj.points); + + // Insert + for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(0, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Insert(Boundary condition) + for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start + 1.1e-3, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(0, x_start + 1.1e-3, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Right on the terminal point + { + auto traj_out = traj; + + const auto p_target = create_point(9.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(0, 9.0, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Not insert(Overlap base_idx point) + for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start + 1e-4, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(0, x_start + 1e-4, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + } + + // Not insert(Overlap base_idx + 1 point) + for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start - 1e-4, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(0, x_start - 1e-4, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + } + + // Invalid target point(In front of the beginning point) + { + auto traj_out = traj; + + const auto insert_idx = insertTargetPoint(0, -1.0, traj_out.points); + + EXPECT_EQ(insert_idx, std::nullopt); + } + + // Invalid target point(Behind the end point) + { + auto traj_out = traj; + + const auto insert_idx = insertTargetPoint(0, 10.0, traj_out.points); + + EXPECT_EQ(insert_idx, std::nullopt); + } + + // Empty + { + auto empty_traj = generateTestTrajectory(0, 1.0); + EXPECT_THROW(insertTargetPoint(0, 0.0, empty_traj.points), std::invalid_argument); + } +} + +TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Idx) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertTargetPoint; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Insert + for (double x_start = 0.5; x_start < 5.0; x_start += 1.0) { + auto traj_out = traj; + + const size_t start_idx = 2; + const auto p_target = create_point(x_start + 2.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(Before End Point) + { + auto traj_out = traj; + const double x_start = 1.0 - 1e-2; + + const size_t start_idx = 8; + const auto p_target = create_point(9.0 - 1e-2, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(Right on End Point) + { + auto traj_out = traj; + const double x_start = 1.0; + + const size_t start_idx = 8; + const auto p_target = create_point(9.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(Insert on end point) + { + auto traj_out = traj; + const double x_start = 4.0; + + const size_t start_idx = 5; + const auto p_target = create_point(9.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(start point) + { + auto traj_out = traj; + const double x_start = 0.0; + + const size_t start_idx = 0; + const auto p_target = create_point(0.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // No Insert (Index Out of range) + { + auto traj_out = traj; + EXPECT_EQ(insertTargetPoint(9, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(9, 1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(10, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(10, 1.0, traj_out.points), std::nullopt); + } + + // No Insert (Length out of range) + { + auto traj_out = traj; + EXPECT_EQ(insertTargetPoint(0, 10.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(0, 9.0001, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(5, 5.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(8, 1.0001, traj_out.points), std::nullopt); + } +} + +TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero_Start_Idx) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertTargetPoint; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Insert + for (double x_start = -0.5; x_start < -5.0; x_start -= 1.0) { + auto traj_out = traj; + + const size_t start_idx = 7; + const auto p_target = create_point(7.0 + x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(Before End Point) + { + auto traj_out = traj; + const double x_start = -1.0 - 1e-2; + + const size_t start_idx = 8; + const auto p_target = create_point(7.0 - 1e-2, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(Right on End Point) + { + auto traj_out = traj; + const double x_start = -1.0; + + const size_t start_idx = 8; + const auto p_target = create_point(7.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(start point) + { + auto traj_out = traj; + const double x_start = -5.0; + + const size_t start_idx = 5; + const auto p_target = create_point(0.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // No Insert (Index Out of range) + { + auto traj_out = traj; + EXPECT_EQ(insertTargetPoint(0, -1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(0, -1.0, traj_out.points), std::nullopt); + } + + // No Insert (Length out of range) + { + auto traj_out = traj; + EXPECT_EQ(insertTargetPoint(9, -10.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(9, -9.0001, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(1, -1.0001, traj_out.points), std::nullopt); + } +} + +TEST(trajectory, insertTargetPoint_Length_from_a_pose) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertTargetPoint; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; + + const auto traj = generateTestTrajectory(10, 1.0); + const auto total_length = calcArcLength(traj.points); + + // Insert (From Zero Point) + for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { + auto traj_out = traj; + + const geometry_msgs::msg::Pose src_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto p_target = create_point(x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Insert (From Non-Zero Point) + { + const double src_pose_x = 5.0; + const double src_pose_y = 3.0; + for (double x_start = 0.5; x_start < total_length - src_pose_x; x_start += 1.0) { + auto traj_out = traj; + + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + } + + // No Insert + { + const double src_pose_x = 2.0; + const double src_pose_y = 3.0; + for (double x_start = 1e-3; x_start < total_length - src_pose_x; x_start += 1.0) { + auto traj_out = traj; + + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + } + } + + // Insert (Boundary Condition) + { + const double src_pose_x = 2.0; + const double src_pose_y = 3.0; + for (double x_start = 1e-2; x_start < total_length - src_pose_x; x_start += 1.0) { + auto traj_out = traj; + + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + } + + // In Front of the beginning point of the trajectory + { + const double src_pose_x = -1.0; + const double src_pose_y = 0.0; + for (double x_start = 0.5 - src_pose_x; x_start < total_length - src_pose_x; x_start += 1.0) { + auto traj_out = traj; + + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + } + + // Insert from the point in front of the trajectory (Invalid) + { + auto traj_out = traj; + const double src_pose_x = -1.0; + const double src_pose_y = 0.0; + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); + EXPECT_EQ(insertTargetPoint(src_pose, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(src_pose, 0.5, traj_out.points), std::nullopt); + } + + // Insert from the point behind the trajectory (Invalid) + { + auto traj_out = traj; + const double src_pose_x = 10.0; + const double src_pose_y = 3.0; + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); + EXPECT_EQ(insertTargetPoint(src_pose, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(src_pose, 1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(src_pose, 10.0, traj_out.points), std::nullopt); + } + + // Insert from the point in front of the trajectory (Boundary Condition) + { + auto traj_out = traj; + const double src_pose_x = -1.0; + const double src_pose_y = 0.0; + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); + const double x_start = -src_pose_x; + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Insert from the end point (Boundary Condition) + { + auto traj_out = traj; + const double src_pose_x = 9.0; + const double src_pose_y = 0.0; + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); + const double x_start = 0.0; + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // No Insert (Negative Insert Length) + { + auto traj_out = traj; + const double src_pose_x = 5.0; + const double src_pose_y = 3.0; + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); + EXPECT_EQ(insertTargetPoint(src_pose, -1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(src_pose, -10.0, traj_out.points), std::nullopt); + } + + // No Insert (Too Far from the source point) + { + auto traj_out = traj; + const double src_pose_x = 5.0; + const double src_pose_y = 3.0; + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); + EXPECT_EQ(insertTargetPoint(src_pose, 1.0, traj_out.points, 1.0), std::nullopt); + EXPECT_EQ(insertTargetPoint(src_pose, 10.0, traj_out.points, 1.0), std::nullopt); + } + + // No Insert (Too large yaw deviation) + { + auto traj_out = traj; + const double src_pose_x = 5.0; + const double src_pose_y = 3.0; + const double src_yaw = deg2rad(60.0); + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, src_yaw); + const double max_dist = std::numeric_limits::max(); + EXPECT_EQ( + insertTargetPoint(src_pose, 1.0, traj_out.points, max_dist, deg2rad(45)), std::nullopt); + EXPECT_EQ( + insertTargetPoint(src_pose, 10.0, traj_out.points, max_dist, deg2rad(45)), std::nullopt); + } +} + +TEST(trajectory, insertStopPoint_from_a_source_index) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertStopPoint; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; + + const auto traj = generateTestTrajectory(10, 1.0, 10.0); + + // Insert + for (double x_start = 0.5; x_start < 5.0; x_start += 1.0) { + auto traj_out = traj; + + const size_t start_idx = 2; + const auto p_target = create_point(x_start + 2.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.value()) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); + } else { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(Before End Point) + { + auto traj_out = traj; + const double x_start = 1.0 - 1e-2; + + const size_t start_idx = 8; + const auto p_target = create_point(9.0 - 1e-2, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.value()) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); + } else { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(Right on End Point) + { + auto traj_out = traj; + const double x_start = 1.0; + + const size_t start_idx = 8; + const auto p_target = create_point(9.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.value()) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); + } else { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(Insert on end point) + { + auto traj_out = traj; + const double x_start = 4.0; + + const size_t start_idx = 5; + const auto p_target = create_point(9.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.value()) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); + } else { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(start point) + { + auto traj_out = traj; + const double x_start = 0.0; + + const size_t start_idx = 0; + const auto p_target = create_point(0.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(start_idx, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.value()) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); + } else { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // No Insert (Index Out of range) + { + auto traj_out = traj; + EXPECT_EQ(insertStopPoint(9, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(9, 1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(10, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(10, 1.0, traj_out.points), std::nullopt); + } + + // No Insert (Length out of range) + { + auto traj_out = traj; + EXPECT_EQ(insertStopPoint(0, 10.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(0, 9.0001, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(5, 5.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(8, 1.0001, traj_out.points), std::nullopt); + } +} + +TEST(trajectory, insertStopPoint_from_front_point) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertStopPoint; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; + + const auto traj = generateTestTrajectory(10, 1.0, 10.0); + + // Insert + for (double x_start = 0.5; x_start < 5.0; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start + 2.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(x_start + 2.0, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.value()) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); + } else { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(Before End Point) + { + auto traj_out = traj; + const double x_start = 1.0 - 1e-2; + + const auto p_target = create_point(9.0 - 1e-2, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(8.0 + x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.value()) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); + } else { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(Right on End Point) + { + auto traj_out = traj; + + const auto p_target = create_point(9.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(9.0, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.value()) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); + } else { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Boundary Condition(start point) + { + auto traj_out = traj; + const double x_start = 0.0; + + const auto p_target = create_point(0.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.value()) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); + } else { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // No Insert (Length out of range) + { + auto traj_out = traj; + EXPECT_EQ(insertStopPoint(9.0 + 1e-2, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(10.0, traj_out.points), std::nullopt); + EXPECT_EQ( + insertStopPoint(-std::numeric_limits::epsilon(), traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(-3.0, traj_out.points), std::nullopt); + } +} + +TEST(trajectory, insertStopPoint_from_a_pose) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertStopPoint; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; + + const auto traj = generateTestTrajectory(10, 1.0, 10.0); + const auto total_length = calcArcLength(traj.points); + + // Insert (From Zero Point) + for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { + auto traj_out = traj; + + const geometry_msgs::msg::Pose src_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const auto p_target = create_point(x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.value()) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); + } else { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Insert (From Non-Zero Point) + { + const double src_pose_x = 5.0; + const double src_pose_y = 3.0; + for (double x_start = 0.5; x_start < total_length - src_pose_x; x_start += 1.0) { + auto traj_out = traj; + + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.value()) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); + } else { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + } + + // No Insert + { + const double src_pose_x = 2.0; + const double src_pose_y = 3.0; + for (double x_start = 1e-3; x_start < total_length - src_pose_x; x_start += 1.0) { + auto traj_out = traj; + + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.value()) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); + } else { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + } + } + } + + // Insert (Boundary Condition) + { + const double src_pose_x = 2.0; + const double src_pose_y = 3.0; + for (double x_start = 1e-2; x_start < total_length - src_pose_x; x_start += 1.0) { + auto traj_out = traj; + + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.value()) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); + } else { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + } + + // In Front of the beginning point of the trajectory + { + const double src_pose_x = -1.0; + const double src_pose_y = 0.0; + for (double x_start = 0.5 - src_pose_x; x_start < total_length - src_pose_x; x_start += 1.0) { + auto traj_out = traj; + + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.value()) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); + } else { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + } + + // Insert from the point in front of the trajectory (Invalid) + { + auto traj_out = traj; + const double src_pose_x = -1.0; + const double src_pose_y = 0.0; + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); + EXPECT_EQ(insertStopPoint(src_pose, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(src_pose, 0.5, traj_out.points), std::nullopt); + } + + // Insert from the point behind the trajectory (Invalid) + { + auto traj_out = traj; + const double src_pose_x = 10.0; + const double src_pose_y = 3.0; + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); + EXPECT_EQ(insertStopPoint(src_pose, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(src_pose, 1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(src_pose, 10.0, traj_out.points), std::nullopt); + } + + // Insert from the point in front of the trajectory (Boundary Condition) + { + auto traj_out = traj; + const double src_pose_x = -1.0; + const double src_pose_y = 0.0; + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); + const double x_start = -src_pose_x; + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.value()) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); + } else { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Insert from the end point (Boundary Condition) + { + auto traj_out = traj; + const double src_pose_x = 9.0; + const double src_pose_y = 0.0; + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); + const double x_start = 0.0; + const auto p_target = create_point(src_pose_x + x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.value()) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); + } else { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // No Insert (Negative Insert Length) + { + auto traj_out = traj; + const double src_pose_x = 5.0; + const double src_pose_y = 3.0; + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); + EXPECT_EQ(insertStopPoint(src_pose, -1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(src_pose, -10.0, traj_out.points), std::nullopt); + } + + // No Insert (Too Far from the source point) + { + auto traj_out = traj; + const double src_pose_x = 5.0; + const double src_pose_y = 3.0; + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); + EXPECT_EQ(insertStopPoint(src_pose, 1.0, traj_out.points, 1.0), std::nullopt); + EXPECT_EQ(insertStopPoint(src_pose, 10.0, traj_out.points, 1.0), std::nullopt); + } + + // No Insert (Too large yaw deviation) + { + auto traj_out = traj; + const double src_pose_x = 5.0; + const double src_pose_y = 3.0; + const double src_yaw = deg2rad(60.0); + const geometry_msgs::msg::Pose src_pose = + createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, src_yaw); + const double max_dist = std::numeric_limits::max(); + EXPECT_EQ(insertStopPoint(src_pose, 1.0, traj_out.points, max_dist, deg2rad(45)), std::nullopt); + EXPECT_EQ( + insertStopPoint(src_pose, 10.0, traj_out.points, max_dist, deg2rad(45)), std::nullopt); + } +} + +TEST(trajectory, insertStopPoint_with_pose_and_segment_index) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertStopPoint; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::deg2rad; + using autoware_utils::get_pose; + + const auto traj = generateTestTrajectory(10, 1.0, 3.0); + const auto total_length = calcArcLength(traj.points); + + // Insert + for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + + for (size_t i = 0; i < insert_idx.value(); ++i) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 3.0, epsilon); + } + for (size_t i = insert_idx.value(); i < traj_out.points.size(); ++i) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Insert(Boundary condition) + for (double x_start = 0.0; x_start < total_length; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start + 1.1e-3, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + for (size_t i = 0; i < insert_idx.value(); ++i) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 3.0, epsilon); + } + for (size_t i = insert_idx.value(); i < traj_out.points.size(); ++i) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Insert(Quaternion interpolation) + for (double x_start = 0.25; x_start < total_length; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start, 0.25 * std::tan(deg2rad(60.0)), 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + for (size_t i = 0; i < insert_idx.value(); ++i) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 3.0, epsilon); + } + for (size_t i = insert_idx.value(); i < traj_out.points.size(); ++i) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + + { + const auto p_insert = get_pose(traj_out.points.at(insert_idx.value())); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0)); + EXPECT_EQ(p_insert.position.x, p_target.x); + EXPECT_EQ(p_insert.position.y, p_target.y); + EXPECT_EQ(p_insert.position.z, p_target.z); + EXPECT_NEAR(p_insert.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_insert.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_insert.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_insert.orientation.w, ans_quat.w, epsilon); + } + + { + const auto p_base = get_pose(traj_out.points.at(base_idx)); + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(60.0)); + EXPECT_NEAR(p_base.orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_base.orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_base.orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_base.orientation.w, ans_quat.w, epsilon); + } + } + + // Not insert(Overlap base_idx point) + for (double x_start = 0.0; x_start < total_length - 1.0 + epsilon; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start + 1e-4, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + for (size_t i = 0; i < insert_idx.value(); ++i) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 3.0, epsilon); + } + for (size_t i = insert_idx.value(); i < traj_out.points.size(); ++i) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + } + + // Not insert(Overlap base_idx + 1 point) + for (double x_start = 1.0; x_start < total_length + epsilon; x_start += 1.0) { + auto traj_out = traj; + + const auto p_target = create_point(x_start - 1e-4, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + } + for (size_t i = 0; i < insert_idx.value(); ++i) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 3.0, epsilon); + } + for (size_t i = insert_idx.value(); i < traj_out.points.size(); ++i) { + EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); + } + } + + // Invalid target point(In front of begin point) + { + auto traj_out = traj; + + const auto p_target = create_point(-1.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); + + EXPECT_EQ(insert_idx, std::nullopt); + } + + // Invalid target point(Behind of end point) + { + auto traj_out = traj; + + const auto p_target = create_point(10.0, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); + + EXPECT_EQ(insert_idx, std::nullopt); + } + + // Invalid target point(Huge lateral offset) + { + auto traj_out = traj; + + const auto p_target = create_point(4.0, 10.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); + + EXPECT_EQ(insert_idx, std::nullopt); + } + + // Invalid base index + { + auto traj_out = traj; + + const size_t segment_idx = 9U; + const auto p_target = create_point(10.0, 0.0, 0.0); + const auto insert_idx = insertStopPoint(segment_idx, p_target, traj_out.points); + + EXPECT_EQ(insert_idx, std::nullopt); + } + + // Empty + { + auto empty_traj = generateTestTrajectory(0, 1.0); + const size_t segment_idx = 0; + EXPECT_FALSE(insertStopPoint(segment_idx, geometry_msgs::msg::Point{}, empty_traj.points)); + } +} + +TEST(trajectory, insertDecelPoint_from_a_point) +{ + using autoware::motion_utils::calcArcLength; + using autoware::motion_utils::findNearestSegmentIndex; + using autoware::motion_utils::insertDecelPoint; + using autoware_utils::calc_distance2d; + using autoware_utils::create_point; + using autoware_utils::get_longitudinal_velocity; + + const auto traj = generateTestTrajectory(10, 1.0, 10.0); + const auto total_length = calcArcLength(traj.points); + const double decel_velocity = 5.0; + + // Insert (From Zero Point) + { + for (double x_start = 0.5; x_start < total_length; x_start += 1.0) { + auto traj_out = traj; + const geometry_msgs::msg::Point src_point = create_point(0.0, 0.0, 0.0); + const auto p_target = create_point(x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertDecelPoint(src_point, x_start, decel_velocity, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.value()) { + EXPECT_NEAR(get_longitudinal_velocity(traj_out.points.at(i)), 10.0, epsilon); + } else { + EXPECT_NEAR(get_longitudinal_velocity(traj_out.points.at(i)), decel_velocity, epsilon); + } + } + } + } + + // Insert (From Non-Zero Point) + { + const double src_point_x = 5.0; + const double src_point_y = 3.0; + for (double x_start = 0.5; x_start < total_length - src_point_x; x_start += 1.0) { + auto traj_out = traj; + const geometry_msgs::msg::Point src_point = create_point(src_point_x, src_point_y, 0.0); + const auto p_target = create_point(src_point_x + x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertDecelPoint(src_point, x_start, decel_velocity, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); + EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.value()) { + EXPECT_NEAR(get_longitudinal_velocity(traj_out.points.at(i)), 10.0, epsilon); + } else { + EXPECT_NEAR(get_longitudinal_velocity(traj_out.points.at(i)), decel_velocity, epsilon); + } + } + } + } + + // No Insert + { + const double src_point_x = 2.0; + const double src_point_y = 3.0; + for (double x_start = 1e-3; x_start < total_length - src_point_x; x_start += 1.0) { + auto traj_out = traj; + const geometry_msgs::msg::Point src_point = create_point(src_point_x, src_point_y, 0.0); + const auto p_target = create_point(src_point_x + x_start, 0.0, 0.0); + const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); + const auto insert_idx = insertDecelPoint(src_point, x_start, decel_velocity, traj_out.points); + + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); + EXPECT_EQ(traj_out.points.size(), traj.points.size()); + + for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { + EXPECT_TRUE(calc_distance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); + if (i < insert_idx.value()) { + EXPECT_NEAR(get_longitudinal_velocity(traj_out.points.at(i)), 10.0, epsilon); + } else { + EXPECT_NEAR(get_longitudinal_velocity(traj_out.points.at(i)), decel_velocity, epsilon); + } + } + } + } +} + +TEST(trajectory, findFirstNearestIndexWithSoftConstraints) +{ + using autoware::motion_utils::findFirstNearestIndexWithSoftConstraints; + using autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; + using autoware_utils::pi; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Non overlapped points + { + // 1. Dist and yaw thresholds are given + // Normal cases + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0, 0.4), + 2U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0, 0.4), + 2U); + + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8), 0.5, 1.0), + 4U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8), 0.5, 1.0), + 4U); + + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0), 1.0, 0.1), + 8U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0), 1.0, 0.1), + 8U); + + // Dist is out of range + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.4), + 2U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.4), + 2U); + + // Yaw is out of range + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0, 0.2), + 2U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0, 0.2), + 2U); + + // Dist and yaw is out of range + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.2), + 2U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.2), + 2U); + + // Empty points + EXPECT_THROW( + findFirstNearestIndexWithSoftConstraints( + Trajectory{}.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.2), + std::invalid_argument); + EXPECT_THROW( + findFirstNearestSegmentIndexWithSoftConstraints( + Trajectory{}.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.2), + std::invalid_argument); + + // 2. Dist threshold is given + // Normal cases + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0), + 2U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 2.0), + 2U); + + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8), 0.5), + 4U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8), 0.5), + 4U); + + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0), 1.0), + 8U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0), 1.0), + 8U); + + // Dist is out of range + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0), + 2U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3), 1.0), + 2U); + + // 3. No threshold is given + // Normal cases + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3)), + 2U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + traj.points, createPose(2.4, 1.3, 0.0, 0.0, 0.0, 0.3)), + 2U); + + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8)), + 4U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + traj.points, createPose(4.1, 0.3, 0.0, 0.0, 0.0, -0.8)), + 4U); + + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0)), + 8U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + traj.points, createPose(8.5, -0.5, 0.0, 0.0, 0.0, 0.0)), + 8U); + } + + // Vertically crossing points + { + // ___ + // | | + // S__|__| + // | + // | + // G + std::vector poses; + poses.push_back(createPose(-2.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + poses.push_back(createPose(-1.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + poses.push_back(createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + poses.push_back(createPose(1.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + poses.push_back(createPose(2.0, 0.0, 0.0, 0.0, 0.0, pi / 2.0)); + poses.push_back(createPose(2.0, 1.0, 0.0, 0.0, 0.0, pi / 2.0)); + poses.push_back(createPose(2.0, 2.0, 0.0, 0.0, 0.0, pi)); + poses.push_back(createPose(1.0, 2.0, 0.0, 0.0, 0.0, pi)); + poses.push_back(createPose(0.0, 2.0, 0.0, 0.0, 0.0, -pi / 2.0)); + poses.push_back(createPose(0.0, 1.0, 0.0, 0.0, 0.0, -pi / 2.0)); + poses.push_back(createPose(0.0, 0.0, 0.0, 0.0, 0.0, -pi / 2.0)); + poses.push_back(createPose(0.0, -1.0, 0.0, 0.0, 0.0, -pi / 2.0)); + poses.push_back(createPose(0.0, -2.0, 0.0, 0.0, 0.0, -pi / 2.0)); + + // 1. Dist and yaw thresholds are given + { + // Normal cases + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 1.0, 0.4), + 2U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 1.0, 0.4), + 2U); + + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, -pi / 2.0), 1.0, 0.4), + 10U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, -pi / 2.0), 1.0, 0.4), + 9U); + + // Several nearest index within threshold + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 10.0, pi * 2.0), + 2U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 10.0, pi * 2.0), + 2U); + + // Dist is out of range + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 0.0, 0.4), + 2U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 0.0, 0.4), + 2U); + + // Yaw is out of range + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.0), + 2U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.3), 1.0, 0.0), + 2U); + + // Dist and yaw is out of range + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.3), 0.0, 0.0), + 2U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.3), 0.0, 0.0), + 2U); + } + + // 2. Dist threshold is given + { + // Normal cases + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 1.0), + 2U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 1.0), + 2U); + + // Several nearest index within threshold + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 10.0), + 2U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 10.0), + 2U); + + // Dist is out of range + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 0.0), + 2U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0), 0.0), + 2U); + } + + // 3. No threshold is given + { + // Normal cases + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints(poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0)), + 2U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(0.3, 0.3, 0.0, 0.0, 0.0, 0.0)), + 2U); + } + } + + { + // Points has a loop structure with the opposite direction (= u-turn) + // __ + // S/G ___|_| + + std::vector poses; + poses.push_back(createPose(-3.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + poses.push_back(createPose(-2.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + poses.push_back(createPose(-1.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + poses.push_back(createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + poses.push_back(createPose(1.0, 0.0, 0.0, 0.0, 0.0, pi / 2.0)); + poses.push_back(createPose(1.0, 1.0, 0.0, 0.0, 0.0, pi)); + poses.push_back(createPose(0.0, 1.0, 0.0, 0.0, 0.0, -pi / 2.0)); + poses.push_back(createPose(0.0, 0.0, 0.0, 0.0, 0.0, pi)); + poses.push_back(createPose(-1.0, 0.0, 0.0, 0.0, 0.0, pi)); + poses.push_back(createPose(-2.0, 0.0, 0.0, 0.0, 0.0, pi)); + poses.push_back(createPose(-3.0, 0.0, 0.0, 0.0, 0.0, pi)); + + // 1. Dist and yaw thresholds are given + { + // Normal cases + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0, 0.4), + 1U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0, 0.4), + 0U); + + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 1.0, 0.4), + 9U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 1.0, 0.4), + 9U); + + // Several nearest index within threshold + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 10.0, pi * 2.0), + 1U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 10.0, pi * 2.0), + 0U); + + // Dist is out of range + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 0.0, 0.4), + 1U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 0.0, 0.4), + 0U); + + // Yaw is out of range + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi * 0.9), 1.0, 0.0), + 1U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi * 0.9), 1.0, 0.0), + 0U); + + // Dist and yaw is out of range + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi * 0.9), 0.0, 0.0), + 1U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi * 0.9), 0.0, 0.0), + 0U); + } + + // 2. Dist threshold is given + { + // Normal cases + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0), + 1U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0), + 0U); + + // Several nearest index within threshold + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 10.0), + 1U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 10.0), + 0U); + + // Dist is out of range + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0), + 1U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0), + 0U); + } + + // 3. No threshold is given + { + // Normal cases + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints(poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0)), + 1U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, 0.0)), + 0U); + } + } + + { // Points has a loop structure with the same direction + // ___ + // | | + // S__|__|__G + std::vector poses; + poses.push_back(createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + poses.push_back(createPose(1.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + poses.push_back(createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + poses.push_back(createPose(3.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + poses.push_back(createPose(4.0, 0.0, 0.0, 0.0, 0.0, pi / 2.0)); + poses.push_back(createPose(4.0, 1.0, 0.0, 0.0, 0.0, pi / 2.0)); + poses.push_back(createPose(4.0, 2.0, 0.0, 0.0, 0.0, pi)); + poses.push_back(createPose(3.0, 2.0, 0.0, 0.0, 0.0, pi)); + poses.push_back(createPose(2.0, 2.0, 0.0, 0.0, 0.0, -pi / 2.0)); + poses.push_back(createPose(2.0, 1.0, 0.0, 0.0, 0.0, -pi / 2.0)); + poses.push_back(createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + poses.push_back(createPose(3.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + poses.push_back(createPose(4.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + poses.push_back(createPose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + poses.push_back(createPose(6.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + + // 1. Dist and yaw thresholds are given + { + // Normal cases + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0, 0.4), + 3U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0, 0.4), + 3U); + + // Several nearest index within threshold + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 10.0, pi * 2.0), + 3U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 10.0, pi * 2.0), + 3U); + + // Dist is out of range + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0, 0.4), + 3U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0, 0.4), + 3U); + + // Yaw is out of range + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0, 0.0), + 3U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0, 0.0), + 3U); + + // Dist and yaw is out of range + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0, 0.0), + 3U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0, 0.0), + 3U); + } + + // 2. Dist threshold is given + { + // Normal cases + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0), + 3U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 1.0), + 3U); + + // Several nearest index within threshold + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 10.0), + 3U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 10.0), + 3U); + + // Dist is out of range + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints( + poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0), + 3U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0), 0.0), + 3U); + } + + // 3. No threshold is given + { + // Normal cases + EXPECT_EQ( + findFirstNearestIndexWithSoftConstraints(poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0)), + 3U); + EXPECT_EQ( + findFirstNearestSegmentIndexWithSoftConstraints( + poses, createPose(3.1, 0.1, 0.0, 0.0, 0.0, 0.0)), + 3U); + } + } +} + +TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointAndSegmentIndex) +{ + using autoware::motion_utils::calcSignedArcLength; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Empty + EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); + + // Same point + { + const auto p = create_point(3.0, 0.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p, 2, p, 2), 0, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, p, 3, p, 3), 0, epsilon); + } + + // Forward + { + const auto p1 = create_point(0.0, 0.0, 0.0); + const auto p2 = create_point(3.0, 1.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 2), 3, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 3), 3, epsilon); + } + + // Backward + { + const auto p1 = create_point(9.0, 0.0, 0.0); + const auto p2 = create_point(8.0, 0.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 8, p2, 7), -1, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 8, p2, 8), -1, epsilon); + } + + // Point before start point + { + const auto p1 = create_point(-3.9, 3.0, 0.0); + const auto p2 = create_point(6.0, -10.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 5), 9.9, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 6), 9.9, epsilon); + } + + // Point after end point + { + const auto p1 = create_point(7.0, -5.0, 0.0); + const auto p2 = create_point(13.3, -10.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 6, p2, 8), 6.3, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 7, p2, 8), 6.3, epsilon); + } + + // Point before start point and after end point + { + const auto p1 = create_point(-4.3, 10.0, 0.0); + const auto p2 = create_point(13.8, -1.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 8), 18.1, epsilon); + } + + // Random cases + { + const auto p1 = create_point(1.0, 3.0, 0.0); + const auto p2 = create_point(9.0, -1.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, p2, 8), 8, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 1, p2, 8), 8, epsilon); + } + { + const auto p1 = create_point(4.3, 7.0, 0.0); + const auto p2 = create_point(2.0, 3.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 4, p2, 2), -2.3, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 4, p2, 1), -2.3, epsilon); + } +} + +TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointIndex) +{ + using autoware::motion_utils::calcSignedArcLength; + + const auto traj = generateTestTrajectory(10, 1.0); + + // Empty + EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); + + // Same point + { + const auto p = create_point(3.0, 0.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p, 2, 3), 0, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, 3, p, 3), 0, epsilon); + } + + // Forward + { + const auto p1 = create_point(0.0, 0.0, 0.0); + const auto p2 = create_point(3.0, 1.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, 3), 3, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, 0, p2, 2), 3, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, 0, p2, 3), 3, epsilon); + } + + // Backward + { + const auto p1 = create_point(9.0, 0.0, 0.0); + const auto p2 = create_point(8.0, 0.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 8, 8), -1, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, 8, p2, 7), 0, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, 8, p2, 8), 0, epsilon); + } + + // Point before start point + { + const auto p1 = create_point(-3.9, 3.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, 6), 9.9, epsilon); + } + + // Point after end point + { + const auto p2 = create_point(13.3, -10.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, 7, p2, 8), 6.3, epsilon); + } + + // Start point + { + const auto p1 = create_point(0.0, 3.0, 0.0); + const auto p2 = create_point(5.3, -10.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, 5), 5, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, 0, p2, 5), 5.3, epsilon); + } + + // Point after end point + { + const auto p1 = create_point(7.3, -5.0, 0.0); + const auto p2 = create_point(9.0, -10.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 7, 9), 1.7, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, 7, p2, 8), 2.0, epsilon); + } + + // Random cases + { + const auto p1 = create_point(1.0, 3.0, 0.0); + const auto p2 = create_point(9.0, -1.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 0, 9), 8, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 1, 9), 8, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, 1, p2, 8), 8, epsilon); + } + { + const auto p1 = create_point(4.3, 7.0, 0.0); + const auto p2 = create_point(2.3, 3.0, 0.0); + EXPECT_NEAR(calcSignedArcLength(traj.points, p1, 4, 2), -2.3, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, 4, p2, 2), -1.7, epsilon); + EXPECT_NEAR(calcSignedArcLength(traj.points, 4, p2, 1), -1.7, epsilon); + } +} + +TEST(trajectory, removeOverlapPoints) +{ + using autoware::motion_utils::removeOverlapPoints; + + const auto traj = generateTestTrajectory(10, 1.0, 1.0); + const auto removed_traj = removeOverlapPoints(traj.points, 0); + EXPECT_EQ(traj.points.size(), removed_traj.size()); + for (size_t i = 0; i < traj.points.size(); ++i) { + EXPECT_NEAR(traj.points.at(i).pose.position.x, removed_traj.at(i).pose.position.x, epsilon); + EXPECT_NEAR(traj.points.at(i).pose.position.y, removed_traj.at(i).pose.position.y, epsilon); + EXPECT_NEAR(traj.points.at(i).pose.position.z, removed_traj.at(i).pose.position.z, epsilon); + EXPECT_NEAR( + traj.points.at(i).pose.orientation.x, removed_traj.at(i).pose.orientation.x, epsilon); + EXPECT_NEAR( + traj.points.at(i).pose.orientation.y, removed_traj.at(i).pose.orientation.y, epsilon); + EXPECT_NEAR( + traj.points.at(i).pose.orientation.z, removed_traj.at(i).pose.orientation.z, epsilon); + EXPECT_NEAR( + traj.points.at(i).pose.orientation.w, removed_traj.at(i).pose.orientation.w, epsilon); + EXPECT_NEAR( + traj.points.at(i).longitudinal_velocity_mps, removed_traj.at(i).longitudinal_velocity_mps, + epsilon); + } + + // No overlap points + { + const auto traj = generateTestTrajectory(10, 1.0, 1.0); + for (size_t start_idx = 0; start_idx < 10; ++start_idx) { + const auto removed_traj = removeOverlapPoints(traj.points, start_idx); + EXPECT_EQ(traj.points.size(), removed_traj.size()); + for (size_t i = 0; i < traj.points.size(); ++i) { + EXPECT_NEAR(traj.points.at(i).pose.position.x, removed_traj.at(i).pose.position.x, epsilon); + EXPECT_NEAR(traj.points.at(i).pose.position.y, removed_traj.at(i).pose.position.y, epsilon); + EXPECT_NEAR(traj.points.at(i).pose.position.z, removed_traj.at(i).pose.position.z, epsilon); + EXPECT_NEAR( + traj.points.at(i).pose.orientation.x, removed_traj.at(i).pose.orientation.x, epsilon); + EXPECT_NEAR( + traj.points.at(i).pose.orientation.y, removed_traj.at(i).pose.orientation.y, epsilon); + EXPECT_NEAR( + traj.points.at(i).pose.orientation.z, removed_traj.at(i).pose.orientation.z, epsilon); + EXPECT_NEAR( + traj.points.at(i).pose.orientation.w, removed_traj.at(i).pose.orientation.w, epsilon); + EXPECT_NEAR( + traj.points.at(i).longitudinal_velocity_mps, removed_traj.at(i).longitudinal_velocity_mps, + epsilon); + } + } + } + + // Overlap points from certain point + { + auto traj = generateTestTrajectory(10, 1.0, 1.0); + traj.points.at(5) = traj.points.at(6); + const auto removed_traj = removeOverlapPoints(traj.points); + + EXPECT_EQ(traj.points.size() - 1, removed_traj.size()); + for (size_t i = 0; i < 6; ++i) { + EXPECT_NEAR(traj.points.at(i).pose.position.x, removed_traj.at(i).pose.position.x, epsilon); + EXPECT_NEAR(traj.points.at(i).pose.position.y, removed_traj.at(i).pose.position.y, epsilon); + EXPECT_NEAR(traj.points.at(i).pose.position.z, removed_traj.at(i).pose.position.z, epsilon); + EXPECT_NEAR( + traj.points.at(i).pose.orientation.x, removed_traj.at(i).pose.orientation.x, epsilon); + EXPECT_NEAR( + traj.points.at(i).pose.orientation.y, removed_traj.at(i).pose.orientation.y, epsilon); + EXPECT_NEAR( + traj.points.at(i).pose.orientation.z, removed_traj.at(i).pose.orientation.z, epsilon); + EXPECT_NEAR( + traj.points.at(i).pose.orientation.w, removed_traj.at(i).pose.orientation.w, epsilon); + EXPECT_NEAR( + traj.points.at(i).longitudinal_velocity_mps, removed_traj.at(i).longitudinal_velocity_mps, + epsilon); + } + + for (size_t i = 6; i < 9; ++i) { + EXPECT_NEAR( + traj.points.at(i + 1).pose.position.x, removed_traj.at(i).pose.position.x, epsilon); + EXPECT_NEAR( + traj.points.at(i + 1).pose.position.y, removed_traj.at(i).pose.position.y, epsilon); + EXPECT_NEAR( + traj.points.at(i + 1).pose.position.z, removed_traj.at(i).pose.position.z, epsilon); + EXPECT_NEAR( + traj.points.at(i + 1).pose.orientation.x, removed_traj.at(i).pose.orientation.x, epsilon); + EXPECT_NEAR( + traj.points.at(i + 1).pose.orientation.y, removed_traj.at(i).pose.orientation.y, epsilon); + EXPECT_NEAR( + traj.points.at(i + 1).pose.orientation.z, removed_traj.at(i).pose.orientation.z, epsilon); + EXPECT_NEAR( + traj.points.at(i + 1).pose.orientation.w, removed_traj.at(i).pose.orientation.w, epsilon); + EXPECT_NEAR( + traj.points.at(i + 1).longitudinal_velocity_mps, + removed_traj.at(i).longitudinal_velocity_mps, epsilon); + } + } + + // Overlap points from certain point + { + auto traj = generateTestTrajectory(10, 1.0, 1.0); + traj.points.at(5) = traj.points.at(6); + const auto removed_traj = removeOverlapPoints(traj.points, 6); + + EXPECT_EQ(traj.points.size(), removed_traj.size()); + for (size_t i = 0; i < traj.points.size(); ++i) { + EXPECT_NEAR(traj.points.at(i).pose.position.x, removed_traj.at(i).pose.position.x, epsilon); + EXPECT_NEAR(traj.points.at(i).pose.position.y, removed_traj.at(i).pose.position.y, epsilon); + EXPECT_NEAR(traj.points.at(i).pose.position.z, removed_traj.at(i).pose.position.z, epsilon); + EXPECT_NEAR( + traj.points.at(i).pose.orientation.x, removed_traj.at(i).pose.orientation.x, epsilon); + EXPECT_NEAR( + traj.points.at(i).pose.orientation.y, removed_traj.at(i).pose.orientation.y, epsilon); + EXPECT_NEAR( + traj.points.at(i).pose.orientation.z, removed_traj.at(i).pose.orientation.z, epsilon); + EXPECT_NEAR( + traj.points.at(i).pose.orientation.w, removed_traj.at(i).pose.orientation.w, epsilon); + EXPECT_NEAR( + traj.points.at(i).longitudinal_velocity_mps, removed_traj.at(i).longitudinal_velocity_mps, + epsilon); + } + } + + // Empty Points + { + const Trajectory traj; + const auto removed_traj = removeOverlapPoints(traj.points); + EXPECT_TRUE(removed_traj.empty()); + } +} + +TEST(trajectory, cropForwardPoints) +{ + using autoware::motion_utils::cropForwardPoints; + + const auto traj = generateTestTrajectory(10, 1.0, 1.0); + + { // Normal case + const auto cropped_traj_points = + cropForwardPoints(traj.points, autoware_utils::create_point(1.5, 1.5, 0.0), 1, 4.8); + EXPECT_EQ(cropped_traj_points.size(), static_cast(7)); + } + + { // Forward length is longer than points arc length. + const auto cropped_traj_points = + cropForwardPoints(traj.points, autoware_utils::create_point(1.5, 1.5, 0.0), 1, 10.0); + EXPECT_EQ(cropped_traj_points.size(), static_cast(10)); + } + + { // Point is on the previous segment + const auto cropped_traj_points = + cropForwardPoints(traj.points, autoware_utils::create_point(1.0, 1.0, 0.0), 0, 2.5); + EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); + } + + { // Point is on the next segment + const auto cropped_traj_points = + cropForwardPoints(traj.points, autoware_utils::create_point(1.0, 1.0, 0.0), 1, 2.5); + EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); + } +} + +TEST(trajectory, cropBackwardPoints) +{ + using autoware::motion_utils::cropBackwardPoints; + + const auto traj = generateTestTrajectory(10, 1.0, 1.0); + + { // Normal case + const auto cropped_traj_points = + cropBackwardPoints(traj.points, autoware_utils::create_point(8.5, 8.5, 0.0), 8, 4.8); + EXPECT_EQ(cropped_traj_points.size(), static_cast(6)); + } + + { // Backward length is longer than points arc length. + const auto cropped_traj_points = + cropBackwardPoints(traj.points, autoware_utils::create_point(8.5, 8.5, 0.0), 8, 10.0); + EXPECT_EQ(cropped_traj_points.size(), static_cast(10)); + } + + { // Point is on the previous segment + const auto cropped_traj_points = + cropBackwardPoints(traj.points, autoware_utils::create_point(8.0, 8.0, 0.0), 7, 2.5); + EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); + } + + { // Point is on the next segment + const auto cropped_traj_points = + cropBackwardPoints(traj.points, autoware_utils::create_point(8.0, 8.0, 0.0), 8, 2.5); + EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); + } +} + +TEST(trajectory, cropPoints) +{ + using autoware::motion_utils::cropPoints; + + const auto traj = generateTestTrajectory(10, 1.0, 1.0); + + { // Normal case + const auto cropped_traj_points = + cropPoints(traj.points, autoware_utils::create_point(3.5, 3.5, 0.0), 3, 2.3, 1.2); + EXPECT_EQ(cropped_traj_points.size(), static_cast(3)); + } + + { // Length is longer than points arc length. + const auto cropped_traj_points = + cropPoints(traj.points, autoware_utils::create_point(3.5, 3.5, 0.0), 3, 10.0, 10.0); + EXPECT_EQ(cropped_traj_points.size(), static_cast(10)); + } + + { // Point is on the previous segment + const auto cropped_traj_points = + cropPoints(traj.points, autoware_utils::create_point(3.0, 3.0, 0.0), 2, 2.2, 1.9); + EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); + } + + { // Point is on the next segment + const auto cropped_traj_points = + cropPoints(traj.points, autoware_utils::create_point(3.0, 3.0, 0.0), 3, 2.2, 1.9); + EXPECT_EQ(cropped_traj_points.size(), static_cast(4)); + } +} + +TEST(Trajectory, removeFirstInvalidOrientationPoints) +{ + using autoware::motion_utils::insertOrientation; + using autoware::motion_utils::removeFirstInvalidOrientationPoints; + + const double max_yaw_diff = M_PI_2; + + auto testRemoveInvalidOrientationPoints = [&]( + const Trajectory & traj, + std::function modifyTrajectory, + size_t expected_size) { + auto modified_traj = traj; + insertOrientation(modified_traj.points, true); + modifyTrajectory(modified_traj); + removeFirstInvalidOrientationPoints(modified_traj.points, max_yaw_diff); + EXPECT_EQ(modified_traj.points.size(), expected_size); + for (size_t i = 0; i < modified_traj.points.size() - 1; ++i) { + EXPECT_EQ(traj.points.at(i), modified_traj.points.at(i)); + const double yaw1 = tf2::getYaw(modified_traj.points.at(i).pose.orientation); + const double yaw2 = tf2::getYaw(modified_traj.points.at(i + 1).pose.orientation); + const double yaw_diff = std::abs(autoware_utils::normalize_radian(yaw1 - yaw2)); + EXPECT_LE(yaw_diff, max_yaw_diff); + } + }; + + auto traj = generateTestTrajectory(10, 1.0, 1.0); + + // no invalid points + testRemoveInvalidOrientationPoints(traj, [](Trajectory &) {}, traj.points.size()); + + // invalid point at the end + testRemoveInvalidOrientationPoints( + traj, + [&](Trajectory & t) { + auto invalid_point = t.points.back(); + invalid_point.pose.orientation = + tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_4)); + t.points.push_back(invalid_point); + }, + traj.points.size()); + + // invalid point in the middle + testRemoveInvalidOrientationPoints( + traj, + [&](Trajectory & t) { + auto invalid_point = t.points[4]; + invalid_point.pose.orientation = + tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_4)); + t.points.insert(t.points.begin() + 4, invalid_point); + }, + traj.points.size()); +} + +TEST(trajectory, calcYawDeviation) +{ + using autoware::motion_utils::calcYawDeviation; + using autoware_planning_msgs::msg::TrajectoryPoint; + + constexpr double tolerance = 1e-3; + + // Generate test trajectory + const auto trajectory = generateTestTrajectory(4, 10.0, 0.0, 0.0, M_PI / 8); + + // check with fist point + { + const auto input_pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + const double yaw_deviation = calcYawDeviation(trajectory.points, input_pose); + constexpr double expected_yaw_deviation = -M_PI / 8; + EXPECT_NEAR(yaw_deviation, expected_yaw_deviation, tolerance); + } + + // check with middle point + { + const auto input_pose = createPose(10.0, 10.0, 0.0, 0.0, 0.0, M_PI / 8); + const double yaw_deviation = calcYawDeviation(trajectory.points, input_pose); + constexpr double expected_yaw_deviation = -0.734; + EXPECT_NEAR(yaw_deviation, expected_yaw_deviation, tolerance); + } +} + +TEST(trajectory, isTargetPointFront) +{ + using autoware::motion_utils::isTargetPointFront; + using autoware_planning_msgs::msg::TrajectoryPoint; + using autoware_utils::create_point; + + // Generate test trajectory + const auto trajectory = generateTestTrajectory(10, 1.0); + + // Front point is base + { + const auto base_point = create_point(5.0, 0.0, 0.0); + const auto target_point = create_point(1.0, 0.0, 0.0); + + EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + // Front point is target + { + const auto base_point = create_point(1.0, 0.0, 0.0); + const auto target_point = create_point(3.0, 0.0, 0.0); + + EXPECT_TRUE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + // boundary condition + { + const auto base_point = create_point(1.0, 0.0, 0.0); + const auto target_point = create_point(1.0, 0.0, 0.0); + + EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + // before the start point + { + const auto base_point = create_point(1.0, 0.0, 0.0); + const auto target_point = create_point(-3.0, 0.0, 0.0); + + EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + { + const auto base_point = create_point(-5.0, 0.0, 0.0); + const auto target_point = create_point(1.0, 0.0, 0.0); + + EXPECT_TRUE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + // after the end point + { + const auto base_point = create_point(10.0, 0.0, 0.0); + const auto target_point = create_point(3.0, 0.0, 0.0); + + EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + { + const auto base_point = create_point(2.0, 0.0, 0.0); + const auto target_point = create_point(14.0, 0.0, 0.0); + + EXPECT_TRUE(isTargetPointFront(trajectory.points, base_point, target_point)); + } + + // empty point + { + const Trajectory traj; + const auto base_point = create_point(2.0, 0.0, 0.0); + const auto target_point = create_point(5.0, 0.0, 0.0); + EXPECT_FALSE(isTargetPointFront(traj.points, base_point, target_point)); + } + + // non-default threshold + { + const double threshold = 3.0; + + { + const auto base_point = create_point(5.0, 0.0, 0.0); + const auto target_point = create_point(3.0, 0.0, 0.0); + + EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point, threshold)); + } + + { + const auto base_point = create_point(1.0, 0.0, 0.0); + const auto target_point = create_point(4.0, 0.0, 0.0); + + EXPECT_FALSE(isTargetPointFront(trajectory.points, base_point, target_point, threshold)); + } + + { + const auto base_point = create_point(1.0, 0.0, 0.0); + const auto target_point = create_point(4.1, 0.0, 0.0); + + EXPECT_TRUE(isTargetPointFront(trajectory.points, base_point, target_point, threshold)); + } + } +} diff --git a/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp b/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp new file mode 100644 index 00000000..e2e88404 --- /dev/null +++ b/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker.cpp @@ -0,0 +1,529 @@ +// 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/motion_utils/vehicle/vehicle_state_checker.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "test_vehicle_state_checker_helper.hpp" + +#include + +#include + +#include +#include + +constexpr double ODOMETRY_HISTORY_500_MS = 0.5; +constexpr double ODOMETRY_HISTORY_1000_MS = 1.0; +constexpr double STOP_DURATION_THRESHOLD_0_MS = 0.0; +constexpr double STOP_DURATION_THRESHOLD_200_MS = 0.2; +constexpr double STOP_DURATION_THRESHOLD_400_MS = 0.4; +constexpr double STOP_DURATION_THRESHOLD_600_MS = 0.6; +constexpr double STOP_DURATION_THRESHOLD_800_MS = 0.8; +constexpr double STOP_DURATION_THRESHOLD_1000_MS = 1.0; + +using autoware::motion_utils::VehicleArrivalChecker; +using autoware::motion_utils::VehicleStopChecker; +using autoware_utils::create_point; +using autoware_utils::create_quaternion; +using autoware_utils::create_translation; +using nav_msgs::msg::Odometry; + +class CheckerNode : public rclcpp::Node +{ +public: + CheckerNode() : Node("test_checker_node") + { + vehicle_stop_checker_ = std::make_unique(this); + vehicle_arrival_checker_ = std::make_unique(this); + } + + std::unique_ptr vehicle_stop_checker_; + std::unique_ptr vehicle_arrival_checker_; +}; + +class PubManager : public rclcpp::Node +{ +public: + PubManager() : Node("test_pub_node") + { + pub_odom_ = create_publisher("/localization/kinematic_state", 1); + pub_traj_ = create_publisher("/planning/scenario_planning/trajectory", 1); + } + + rclcpp::Publisher::SharedPtr pub_odom_; + rclcpp::Publisher::SharedPtr pub_traj_; + + void publishStoppedOdometry(const geometry_msgs::msg::Pose & pose, const double publish_duration) + { + const auto start_time = this->now(); + while (true) { + const auto now = this->now(); + + const auto time_diff = now - start_time; + if (publish_duration < time_diff.seconds()) { + break; + } + + Odometry odometry; + odometry.header.stamp = now; + odometry.pose.pose = pose; + odometry.twist.twist.linear = create_translation(0.0, 0.0, 0.0); + odometry.twist.twist.angular = create_translation(0.0, 0.0, 0.0); + this->pub_odom_->publish(odometry); + + rclcpp::WallRate(10).sleep(); + } + } + + void publishStoppedOdometry(const double publish_duration) + { + const auto start_time = this->now(); + while (true) { + const auto now = this->now(); + + const auto time_diff = now - start_time; + if (publish_duration < time_diff.seconds()) { + break; + } + + Odometry odometry; + odometry.header.stamp = now; + odometry.pose.pose.position = create_point(0.0, 0.0, 0.0); + odometry.pose.pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0); + odometry.twist.twist.linear = create_translation(0.0, 0.0, 0.0); + odometry.twist.twist.angular = create_translation(0.0, 0.0, 0.0); + this->pub_odom_->publish(odometry); + + rclcpp::WallRate(10).sleep(); + } + } + + void publishStoppingOdometry(const double publish_duration) + { + const auto start_time = this->now(); + while (true) { + const auto now = this->now(); + + const auto time_diff = now - start_time; + if (publish_duration < time_diff.seconds()) { + break; + } + + Odometry odometry; + odometry.header.stamp = now; + odometry.pose.pose.position = create_point(0.0, 0.0, 0.0); + odometry.pose.pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0); + odometry.twist.twist.linear = time_diff.seconds() < publish_duration / 2.0 + ? create_translation(1.0, 0.0, 0.0) + : create_translation(0.0, 0.0, 0.0); + odometry.twist.twist.angular = create_translation(0.0, 0.0, 0.0); + this->pub_odom_->publish(odometry); + + rclcpp::WallRate(10).sleep(); + } + } + + void publishOldOdometry(const double publish_duration) + { + const auto start_time = this->now(); + while (true) { + const auto now = this->now(); + + const auto time_diff = now - start_time; + if (publish_duration < time_diff.seconds()) { + break; + } + + Odometry odometry; + odometry.header.stamp = now - rclcpp::Duration(15, 0); // 15 seconds old data + odometry.pose.pose.position = create_point(0.0, 0.0, 0.0); + odometry.pose.pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0); + odometry.twist.twist.linear = create_translation(0.0, 0.0, 0.0); + odometry.twist.twist.angular = create_translation(0.0, 0.0, 0.0); + this->pub_odom_->publish(odometry); + + rclcpp::WallRate(10).sleep(); + } + } +}; + +TEST(vehicle_stop_checker, isVehicleStopped) +{ + { + auto checker = std::make_shared(); + auto manager = std::make_shared(); + EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected."; + + EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped()); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(checker); + executor.add_node(manager); + + std::thread spin_thread = + std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); + + manager->publishStoppedOdometry(ODOMETRY_HISTORY_500_MS); + + EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped()); + EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS)); + EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS)); + EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_400_MS)); + EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_600_MS)); + EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_800_MS)); + EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_1000_MS)); + + executor.cancel(); + spin_thread.join(); + checker.reset(); + manager.reset(); + } + + { + auto checker = std::make_shared(); + auto manager = std::make_shared(); + EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected."; + + EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped()); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(checker); + executor.add_node(manager); + + std::thread spin_thread = + std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); + + manager->publishStoppedOdometry(ODOMETRY_HISTORY_1000_MS); + + EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped()); + EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS)); + EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS)); + EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_400_MS)); + EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_600_MS)); + EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_800_MS)); + EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_1000_MS)); + + executor.cancel(); + spin_thread.join(); + checker.reset(); + manager.reset(); + } + + { + auto checker = std::make_shared(); + auto manager = std::make_shared(); + EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected."; + + EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped()); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(checker); + executor.add_node(manager); + + std::thread spin_thread = + std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); + + manager->publishStoppingOdometry(ODOMETRY_HISTORY_1000_MS); + + EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped()); + EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS)); + EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS)); + EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_400_MS)); + EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_600_MS)); + EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_800_MS)); + EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_1000_MS)); + + executor.cancel(); + spin_thread.join(); + checker.reset(); + manager.reset(); + } + + // check if the old data will be discarded + { + auto checker = std::make_shared(); + auto manager = std::make_shared(); + EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected."; + + EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped()); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(checker); + executor.add_node(manager); + + std::thread spin_thread = + std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); + + manager->publishOldOdometry(ODOMETRY_HISTORY_500_MS); + + EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped()); + EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS)); + EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS)); + EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_400_MS)); + EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_600_MS)); + EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_800_MS)); + EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_1000_MS)); + + executor.cancel(); + spin_thread.join(); + checker.reset(); + manager.reset(); + } +} + +TEST(vehicle_arrival_checker, isVehicleStopped) +{ + { + auto checker = std::make_shared(); + auto manager = std::make_shared(); + EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected."; + + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStopped()); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(checker); + executor.add_node(manager); + + std::thread spin_thread = + std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); + + manager->publishStoppedOdometry(ODOMETRY_HISTORY_500_MS); + + EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStopped()); + EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS)); + EXPECT_TRUE( + checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS)); + EXPECT_TRUE( + checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_400_MS)); + EXPECT_FALSE( + checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_600_MS)); + EXPECT_FALSE( + checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_800_MS)); + EXPECT_FALSE( + checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_1000_MS)); + + executor.cancel(); + spin_thread.join(); + checker.reset(); + manager.reset(); + } + + { + auto checker = std::make_shared(); + auto manager = std::make_shared(); + EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected."; + + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStopped()); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(checker); + executor.add_node(manager); + + std::thread spin_thread = + std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); + + manager->publishStoppedOdometry(ODOMETRY_HISTORY_1000_MS); + + EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStopped()); + EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS)); + EXPECT_TRUE( + checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS)); + EXPECT_TRUE( + checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_400_MS)); + EXPECT_TRUE( + checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_600_MS)); + EXPECT_TRUE( + checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_800_MS)); + EXPECT_TRUE( + checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_1000_MS)); + + executor.cancel(); + spin_thread.join(); + checker.reset(); + manager.reset(); + } + + { + auto checker = std::make_shared(); + auto manager = std::make_shared(); + EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected."; + + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStopped()); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(checker); + executor.add_node(manager); + + std::thread spin_thread = + std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); + + manager->publishStoppingOdometry(ODOMETRY_HISTORY_1000_MS); + + EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStopped()); + EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS)); + EXPECT_TRUE( + checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS)); + EXPECT_TRUE( + checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_400_MS)); + EXPECT_FALSE( + checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_600_MS)); + EXPECT_FALSE( + checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_800_MS)); + EXPECT_FALSE( + checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_1000_MS)); + + executor.cancel(); + spin_thread.join(); + checker.reset(); + manager.reset(); + } +} + +TEST(vehicle_arrival_checker, isVehicleStoppedAtStopPoint) +{ + { + auto checker = std::make_shared(); + auto manager = std::make_shared(); + EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected."; + + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStopped()); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(checker); + executor.add_node(manager); + + std::thread spin_thread = + std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); + + geometry_msgs::msg::Pose odom_pose; + odom_pose.position = create_point(10.0, 0.0, 0.0); + odom_pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0); + + geometry_msgs::msg::Pose goal_pose; + goal_pose.position = create_point(10.0, 0.0, 0.0); + goal_pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0); + + manager->pub_traj_->publish(generateTrajectoryWithStopPoint(goal_pose)); + manager->publishStoppedOdometry(odom_pose, ODOMETRY_HISTORY_500_MS); + + EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint()); + EXPECT_TRUE( + checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(STOP_DURATION_THRESHOLD_0_MS)); + EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( + STOP_DURATION_THRESHOLD_200_MS)); + EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( + STOP_DURATION_THRESHOLD_400_MS)); + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( + STOP_DURATION_THRESHOLD_600_MS)); + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( + STOP_DURATION_THRESHOLD_800_MS)); + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( + STOP_DURATION_THRESHOLD_1000_MS)); + + executor.cancel(); + spin_thread.join(); + checker.reset(); + manager.reset(); + } + + { + auto checker = std::make_shared(); + auto manager = std::make_shared(); + EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected."; + + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint()); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(checker); + executor.add_node(manager); + + std::thread spin_thread = + std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); + + geometry_msgs::msg::Pose odom_pose; + odom_pose.position = create_point(0.0, 0.0, 0.0); + odom_pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0); + + geometry_msgs::msg::Pose goal_pose; + goal_pose.position = create_point(10.0, 0.0, 0.0); + goal_pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0); + + manager->pub_traj_->publish(generateTrajectoryWithStopPoint(goal_pose)); + manager->publishStoppedOdometry(odom_pose, ODOMETRY_HISTORY_500_MS); + + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint()); + EXPECT_FALSE( + checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(STOP_DURATION_THRESHOLD_0_MS)); + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( + STOP_DURATION_THRESHOLD_200_MS)); + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( + STOP_DURATION_THRESHOLD_400_MS)); + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( + STOP_DURATION_THRESHOLD_600_MS)); + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( + STOP_DURATION_THRESHOLD_800_MS)); + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( + STOP_DURATION_THRESHOLD_1000_MS)); + + executor.cancel(); + spin_thread.join(); + checker.reset(); + manager.reset(); + } + + { + auto checker = std::make_shared(); + auto manager = std::make_shared(); + EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected."; + + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint()); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(checker); + executor.add_node(manager); + + std::thread spin_thread = + std::thread(std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, &executor)); + + geometry_msgs::msg::Pose odom_pose; + odom_pose.position = create_point(10.0, 0.0, 0.0); + odom_pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0); + + geometry_msgs::msg::Pose goal_pose; + goal_pose.position = create_point(10.0, 0.0, 0.0); + goal_pose.orientation = create_quaternion(0.0, 0.0, 0.0, 1.0); + + manager->pub_traj_->publish(generateTrajectoryWithoutStopPoint(goal_pose)); + manager->publishStoppedOdometry(odom_pose, ODOMETRY_HISTORY_500_MS); + + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint()); + EXPECT_FALSE( + checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(STOP_DURATION_THRESHOLD_0_MS)); + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( + STOP_DURATION_THRESHOLD_200_MS)); + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( + STOP_DURATION_THRESHOLD_400_MS)); + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( + STOP_DURATION_THRESHOLD_600_MS)); + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( + STOP_DURATION_THRESHOLD_800_MS)); + EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint( + STOP_DURATION_THRESHOLD_1000_MS)); + + executor.cancel(); + spin_thread.join(); + checker.reset(); + manager.reset(); + } +} diff --git a/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp b/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp new file mode 100644 index 00000000..29802e70 --- /dev/null +++ b/common/autoware_motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp @@ -0,0 +1,59 @@ +// 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 VEHICLE__TEST_VEHICLE_STATE_CHECKER_HELPER_HPP_ +#define VEHICLE__TEST_VEHICLE_STATE_CHECKER_HELPER_HPP_ + +#include +#include + +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; + +inline Trajectory generateTrajectoryWithStopPoint(const geometry_msgs::msg::Pose & goal_pose) +{ + constexpr double interval_distance = 1.0; + + Trajectory traj; + for (double s = 0.0; s <= 10.0 * interval_distance; s += interval_distance) { + TrajectoryPoint p; + p.pose = goal_pose; + p.pose.position.x += s; + p.longitudinal_velocity_mps = 1.0; + traj.points.push_back(p); + } + + traj.points.front().longitudinal_velocity_mps = 0.0; + std::reverse(traj.points.begin(), traj.points.end()); + return traj; +} + +inline Trajectory generateTrajectoryWithoutStopPoint(const geometry_msgs::msg::Pose & goal_pose) +{ + constexpr double interval_distance = 1.0; + + Trajectory traj; + for (double s = 0.0; s <= 10.0 * interval_distance; s += interval_distance) { + TrajectoryPoint p; + p.pose = goal_pose; + p.pose.position.x += s; + p.longitudinal_velocity_mps = 1.0; + traj.points.push_back(p); + } + + std::reverse(traj.points.begin(), traj.points.end()); + return traj; +} + +#endif // VEHICLE__TEST_VEHICLE_STATE_CHECKER_HELPER_HPP_