From 32313df8bff83d1605b120af41287be54872a50d Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Wed, 6 Nov 2024 12:58:33 +0900 Subject: [PATCH] feat(autoware_motion_utils): add new trajectory class (#8693) * feat(autoware_motion_utils): add interpolator Signed-off-by: Y.Hisaki * use int32_t instead of int Signed-off-by: Y.Hisaki * use int32_t instead of int Signed-off-by: Y.Hisaki * use int32_t instead of int Signed-off-by: Y.Hisaki * add const as much as possible and use `at()` in `vector` Signed-off-by: Y.Hisaki * fix directory name Signed-off-by: Y.Hisaki * refactor code and add example Signed-off-by: Y.Hisaki * update Signed-off-by: Y.Hisaki * remove unused include Signed-off-by: Y.Hisaki * refactor code Signed-off-by: Y.Hisaki * add clone function Signed-off-by: Y.Hisaki * fix stairstep Signed-off-by: Y.Hisaki * make constructor to public Signed-off-by: Y.Hisaki * feat(autoware_motion_utils): add trajectory class Signed-off-by: Y.Hisaki * Update CMakeLists.txt * fix Signed-off-by: Y.Hisaki * fix package.xml Signed-off-by: Y.Hisaki * update crop Signed-off-by: Y.Hisaki * revert crtp change Signed-off-by: Y.Hisaki * update package.xml Signed-off-by: Y.Hisaki * updating... Signed-off-by: Y.Hisaki * update Signed-off-by: Y.Hisaki * solve build problem Signed-off-by: Y.Hisaki * style(pre-commit): autofix --------- Signed-off-by: Y.Hisaki Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../examples/example_interpolator.cpp | 5 +- .../example_trajectory_path_point.cpp | 112 +++++++ .../examples/example_trajectory_point.cpp | 133 ++++++++ .../examples/example_trajectory_pose.cpp | 93 ++++++ .../trajectory_container/trajectory.hpp | 21 ++ .../trajectory/detail/interpolated_array.hpp | 210 +++++++++++++ .../trajectory/detail/utils.hpp | 75 +++++ .../trajectory/trajectory_path_point.hpp | 146 +++++++++ .../trajectory_path_point_with_lane_id.hpp | 154 +++++++++ .../trajectory/trajectory_point.hpp | 297 ++++++++++++++++++ .../trajectory/trajectory_pose.hpp | 121 +++++++ common/autoware_motion_utils/package.xml | 1 + .../trajectory/detail/util.cpp | 132 ++++++++ .../trajectory/trajectory_path_point.cpp | 89 ++++++ .../trajectory_path_point_with_lane_id.cpp | 73 +++++ .../trajectory/trajectory_point.cpp | 134 ++++++++ .../trajectory/trajectory_pose.cpp | 102 ++++++ .../test_trajectory_container.cpp | 198 ++++++++++++ 18 files changed, 2093 insertions(+), 3 deletions(-) create mode 100644 common/autoware_motion_utils/examples/example_trajectory_path_point.cpp create mode 100644 common/autoware_motion_utils/examples/example_trajectory_point.cpp create mode 100644 common/autoware_motion_utils/examples/example_trajectory_pose.cpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/detail/interpolated_array.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/detail/utils.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point_with_lane_id.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_point.hpp create mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_pose.hpp create mode 100644 common/autoware_motion_utils/src/trajectory_container/trajectory/detail/util.cpp create mode 100644 common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_path_point.cpp create mode 100644 common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_path_point_with_lane_id.cpp create mode 100644 common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_point.cpp create mode 100644 common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_pose.cpp create mode 100644 common/autoware_motion_utils/test/src/trajectory_container/test_trajectory_container.cpp diff --git a/common/autoware_motion_utils/examples/example_interpolator.cpp b/common/autoware_motion_utils/examples/example_interpolator.cpp index 6ce81e4572276..6c17975c74359 100644 --- a/common/autoware_motion_utils/examples/example_interpolator.cpp +++ b/common/autoware_motion_utils/examples/example_interpolator.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// 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. @@ -17,8 +17,7 @@ #include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" #include "autoware/motion_utils/trajectory_container/interpolator/linear.hpp" #include "autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp" - -#include +#include "autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp" #include diff --git a/common/autoware_motion_utils/examples/example_trajectory_path_point.cpp b/common/autoware_motion_utils/examples/example_trajectory_path_point.cpp new file mode 100644 index 0000000000000..2f5aedc9c11e1 --- /dev/null +++ b/common/autoware_motion_utils/examples/example_trajectory_path_point.cpp @@ -0,0 +1,112 @@ +// 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_container/trajectory/trajectory_path_point.hpp" + +#include +#include + +#include + +#include + +autoware_planning_msgs::msg::PathPoint pose(double x, double y) +{ + autoware_planning_msgs::msg::PathPoint p; + p.pose.position.x = x; + p.pose.position.y = y; + p.longitudinal_velocity_mps = 1.0; + return p; +} + +int main() +{ + using autoware::motion_utils::trajectory_container::trajectory::TrajectoryContainer; + + pybind11::scoped_interpreter guard{}; + + auto plt = matplotlibcpp17::pyplot::import(); + + std::vector points = { + pose(0.49, 0.59), pose(0.61, 1.22), pose(0.86, 1.93), pose(1.20, 2.56), pose(1.51, 3.17), + pose(1.85, 3.76), pose(2.14, 4.26), pose(2.60, 4.56), pose(3.07, 4.55), pose(3.61, 4.30), + pose(3.95, 4.01), pose(4.29, 3.68), pose(4.90, 3.25), pose(5.54, 3.10), pose(6.24, 3.18), + pose(6.88, 3.54), pose(7.51, 4.25), pose(7.85, 4.93), pose(8.03, 5.73), pose(8.16, 6.52), + pose(8.31, 7.28), pose(8.45, 7.93), pose(8.68, 8.45), pose(8.96, 8.96), pose(9.32, 9.36)}; + + { + std::vector x; + std::vector y; + + for (const auto & p : points) { + x.push_back(p.pose.position.x); + y.push_back(p.pose.position.y); + } + + plt.scatter(Args(x, y), Kwargs("label"_a = "Original", "color"_a = "red")); + } + + auto trajectory = + TrajectoryContainer::Builder{}.build(points); + + if (!trajectory) { + std::cerr << "Failed to build trajectory" << std::endl; + return 1; + } + + trajectory->align_orientation_with_trajectory_direction(); + + geometry_msgs::msg::Point p1; + geometry_msgs::msg::Point p2; + p1.x = 7.5; + p1.y = 8.6; + p2.x = 10.2; + p2.y = 7.7; + + auto s = trajectory->crossed(p1, p2); + auto crossed = trajectory->compute(s.value()); + + plt.plot( + Args(std::vector{p1.x, p2.x}, std::vector{p1.y, p2.y}), + Kwargs("color"_a = "purple")); + + plt.scatter( + Args(crossed.pose.position.x, crossed.pose.position.y), + Kwargs("label"_a = "Crossed on trajectory", "color"_a = "purple")); + + trajectory->longitudinal_velocity_mps(*s, trajectory->length()) = 0.0; + + std::vector x; + std::vector y; + std::vector x_stopped; + std::vector y_stopped; + + for (double i = 0.0; i <= trajectory->length(); i += 0.005) { + auto p = trajectory->compute(i); + if (p.longitudinal_velocity_mps > 0.0) { + x.push_back(p.pose.position.x); + y.push_back(p.pose.position.y); + } else { + x_stopped.push_back(p.pose.position.x); + y_stopped.push_back(p.pose.position.y); + } + } + + plt.plot(Args(x, y), Kwargs("label"_a = "Trajectory", "color"_a = "blue")); + plt.plot(Args(x_stopped, y_stopped), Kwargs("label"_a = "Stopped", "color"_a = "orange")); + + plt.axis(Args("equal")); + plt.legend(); + plt.show(); +} diff --git a/common/autoware_motion_utils/examples/example_trajectory_point.cpp b/common/autoware_motion_utils/examples/example_trajectory_point.cpp new file mode 100644 index 0000000000000..42c915896e460 --- /dev/null +++ b/common/autoware_motion_utils/examples/example_trajectory_point.cpp @@ -0,0 +1,133 @@ +// 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_container/interpolator/cubic_spline.hpp" +#include "autoware/motion_utils/trajectory_container/trajectory/trajectory_point.hpp" + +#include + +#include + +#include +#include + +geometry_msgs::msg::Point pose(double x, double y) +{ + geometry_msgs::msg::Point p; + p.x = x; + p.y = y; + return p; +} + +int main() +{ + pybind11::scoped_interpreter guard{}; + + auto plt = matplotlibcpp17::pyplot::import(); + + std::vector points = { + pose(0.49, 0.59), pose(0.61, 1.22), pose(0.86, 1.93), pose(1.20, 2.56), pose(1.51, 3.17), + pose(1.85, 3.76), pose(2.14, 4.26), pose(2.60, 4.56), pose(3.07, 4.55), pose(3.61, 4.30), + pose(3.95, 4.01), pose(4.29, 3.68), pose(4.90, 3.25), pose(5.54, 3.10), pose(6.24, 3.18), + pose(6.88, 3.54), pose(7.51, 4.25), pose(7.85, 4.93), pose(8.03, 5.73), pose(8.16, 6.52), + pose(8.31, 7.28), pose(8.45, 7.93), pose(8.68, 8.45), pose(8.96, 8.96), pose(9.32, 9.36)}; + + { + std::vector x; + std::vector y; + + for (const auto & p : points) { + x.push_back(p.x); + y.push_back(p.y); + } + + plt.scatter(Args(x, y), Kwargs("label"_a = "Original", "color"_a = "red")); + } + + using autoware::motion_utils::trajectory_container::interpolator::CubicSpline; + using autoware::motion_utils::trajectory_container::trajectory::TrajectoryContainer; + + auto trajectory = TrajectoryContainer::Builder() + .set_xy_interpolator() + .set_z_interpolator() + .build(points); + std::cout << "Trajectory length: " << trajectory->length() << std::endl; + + trajectory->crop(2.0, trajectory->length() - 4.0); + + std::cout << "Trajectory length after cropping: " << trajectory->length() << std::endl; + + { + std::vector x; + std::vector y; + + for (double i = 0.0; i <= trajectory->length(); i += 0.01) { + auto p = trajectory->compute(i); + x.push_back(p.x); + y.push_back(p.y); + } + + plt.plot(Args(x, y), Kwargs("label"_a = "Trajectory", "color"_a = "blue")); + } + + { + geometry_msgs::msg::Point p; + p.x = 5.37; + p.y = 6.0; + + auto s = trajectory->closest(p); + auto closest = trajectory->compute(s); + + plt.scatter(Args(p.x, p.y), Kwargs("color"_a = "green")); + plt.scatter( + Args(closest.x, closest.y), Kwargs("label"_a = "Closest on trajectory", "color"_a = "green")); + + plt.plot( + Args(std::vector{p.x, closest.x}, std::vector{p.y, closest.y}), + Kwargs("color"_a = "green")); + } + { + geometry_msgs::msg::Point p1; + geometry_msgs::msg::Point p2; + p1.x = 6.97; + p1.y = 6.36; + p2.x = 9.23; + p2.y = 5.92; + + auto s = trajectory->crossed(p1, p2); + auto crossed = trajectory->compute(s.value()); + + plt.plot( + Args(std::vector{p1.x, p2.x}, std::vector{p1.y, p2.y}), + Kwargs("color"_a = "purple")); + + plt.scatter( + Args(crossed.x, crossed.y), + Kwargs("label"_a = "Crossed on trajectory", "color"_a = "purple")); + } + { + auto restored = trajectory->restore(50); + std::vector x; + std::vector y; + for (const auto & p : restored) { + x.push_back(p.x); + y.push_back(p.y); + } + plt.scatter(Args(x, y), Kwargs("label"_a = "Restored", "color"_a = "orange", "marker"_a = "x")); + } + + plt.axis(Args("equal")); + plt.legend(); + plt.show(); +} diff --git a/common/autoware_motion_utils/examples/example_trajectory_pose.cpp b/common/autoware_motion_utils/examples/example_trajectory_pose.cpp new file mode 100644 index 0000000000000..678176c39e99c --- /dev/null +++ b/common/autoware_motion_utils/examples/example_trajectory_pose.cpp @@ -0,0 +1,93 @@ +// 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_container/trajectory/trajectory_pose.hpp" + +#include + +#include +#include +#include + +#include + +geometry_msgs::msg::Pose pose(double x, double y) +{ + geometry_msgs::msg::Pose p; + p.position.x = x; + p.position.y = y; + p.position.z = 0.0; + return p; +} + +int main() +{ + pybind11::scoped_interpreter guard{}; + + auto plt = matplotlibcpp17::pyplot::import(); + + std::vector poses = { + pose(0.49, 0.59), pose(0.61, 1.22), pose(0.86, 1.93), pose(1.20, 2.56), pose(1.51, 3.17), + pose(1.85, 3.76), pose(2.14, 4.26), pose(2.60, 4.56), pose(3.07, 4.55), pose(3.61, 4.30), + pose(3.95, 4.01), pose(4.29, 3.68), pose(4.90, 3.25), pose(5.54, 3.10), pose(6.24, 3.18), + pose(6.88, 3.54), pose(7.51, 4.25), pose(7.85, 4.93), pose(8.03, 5.73), pose(8.16, 6.52), + pose(8.31, 7.28), pose(8.45, 7.93), pose(8.68, 8.45), pose(8.96, 8.96), pose(9.32, 9.36)}; + + using autoware::motion_utils::trajectory_container::interpolator::CubicSpline; + using autoware::motion_utils::trajectory_container::trajectory::TrajectoryContainer; + + auto trajectory = TrajectoryContainer::Builder{}.build(poses); + + trajectory->align_orientation_with_trajectory_direction(); + + { + std::vector x; + std::vector y; + + for (double i = 0.0; i <= trajectory->length(); i += 0.01) { + auto p = trajectory->compute(i); + x.push_back(p.position.x); + y.push_back(p.position.y); + } + + plt.plot(Args(x, y), Kwargs("label"_a = "Trajectory", "color"_a = "blue")); + } + + { + std::vector x; + std::vector y; + std::vector dx; + std::vector dy; + + for (double i = 0.0; i <= trajectory->length(); i += 1.0) { + auto p = trajectory->compute(i); + x.push_back(p.position.x); + y.push_back(p.position.y); + + tf2::Vector3 x_axis(1.0, 0.0, 0.0); + tf2::Quaternion q(p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w); + tf2::Vector3 direction = tf2::quatRotate(q, x_axis); + dx.push_back(direction.x()); + dy.push_back(direction.y()); + // double azimuth = trajectory->azimuth(i); + // dx.push_back(std::cos(azimuth)); + // dy.push_back(std::sin(azimuth)); + } + plt.quiver(Args(x, y, dx, dy), Kwargs("label"_a = "Direction", "color"_a = "green")); + } + + plt.axis(Args("equal")); + plt.legend(); + plt.show(); +} diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory.hpp new file mode 100644 index 0000000000000..2e56dcf5b5521 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory.hpp @@ -0,0 +1,21 @@ +// 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_CONTAINER__TRAJECTORY_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY_HPP_ +#include +#include +#include +#include +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/detail/interpolated_array.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/detail/interpolated_array.hpp new file mode 100644 index 0000000000000..e00d1aba61e34 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/detail/interpolated_array.hpp @@ -0,0 +1,210 @@ +// 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. + +// clang-format off +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ // NOLINT +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ // NOLINT +// clang-format on + +#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace autoware::motion_utils::trajectory_container::trajectory::detail +{ + +/** + * @brief Class representing an array with interpolatable values that can be manipulated. + * @tparam T The type of values stored in the array. + */ +template +class InterpolatedArray +{ + using InterpolatorType = interpolator::InterpolatorInterface; + +private: + std::vector bases_; + std::vector values_; + std::shared_ptr> interpolator_; + +public: + /** + * @brief Construct a InterpolatedArray with a given interpolator. + * @param interpolator Shared pointer to the interpolator. + */ + explicit InterpolatedArray(const std::shared_ptr & interpolator) + : interpolator_(interpolator) + { + } + + /** + * @brief Copy constructor. + * @param other The InterpolatedArray to copy from. + */ + InterpolatedArray(const InterpolatedArray & other) + : bases_(other.bases_), values_(other.values_), interpolator_(other.interpolator_->clone()) + { + } + + InterpolatedArray(InterpolatedArray && other) = default; + + bool build(const std::vector & bases, const std::vector & values) + { + bases_ = bases; + values_ = values; + return interpolator_->build(bases_, values_); + } + + /** + * @brief Move constructor. + * @param other The InterpolatedArray to move from. + */ + InterpolatedArray & operator=(InterpolatedArray && other) = default; + + /** + * @brief Copy assignment operator. + * @param other The InterpolatedArray to copy from. + * @return Reference to this InterpolatedArray. + */ + InterpolatedArray & operator=(const InterpolatedArray & other) + { + bases_ = other.bases_; + values_ = other.values_; + interpolator_ = other.interpolator_->clone(); + return *this; + } + + // Destructor + ~InterpolatedArray() = default; + + /** + * @brief Get the start value of the base. + * @return The start value. + */ + [[nodiscard]] double start() const { return bases_.front(); } + + /** + * @brief Get the end value of the base. + * @return The end value. + */ + [[nodiscard]] double end() const { return bases_.at(bases_.size() - 1); } + + class Segment + { + friend class InterpolatedArray; + + double start_; + double end_; + InterpolatedArray & parent_; + Segment(InterpolatedArray & parent, double start, double end) + : start_(start), end_(end), parent_(parent) + { + } + + public: + auto & operator=(const T & value) + { + std::vector & bases = parent_.bases_; + std::vector & values = parent_.values_; + + auto insert_if_not_present = [&](double val) -> size_t { + auto it = std::lower_bound(bases.begin(), bases.end(), val); + size_t index = std::distance(bases.begin(), it); + + if (it != bases.end() && *it == val) { + // Return the index if the value already exists + return index; + } // Insert into bases + bases.insert(it, val); + // Insert into values at the corresponding position + values.insert(values.begin() + index, value); + return index; + }; + + // Insert the start value if not present + size_t start_index = insert_if_not_present(start_); + + // Insert the end value if not present + size_t end_index = insert_if_not_present(end_); + + // Ensure the indices are in ascending order + if (start_index > end_index) { + std::swap(start_index, end_index); + } + + // Set the values in the specified range + std::fill(values.begin() + start_index, values.begin() + end_index + 1, value); + + parent_.interpolator_->build(bases, values); + + return *this; + } + }; + + /** + * @brief Get a Segment object to set values in a specific range. + * @param start Start of the range. + * @param end End of the range. + * @return RangeSetter object. + */ + Segment operator()(double start, double end) + { + if (start < this->start() || end > this->end()) { + RCLCPP_WARN( + rclcpp::get_logger("InterpolatedArray"), + "The range [%f, %f] is out of the array range [%f, %f]", start, end, this->start(), + this->end()); + start = std::max(start, this->start()); + end = std::min(end, this->end()); + } + return Segment{*this, start, end}; + } + + /** + * @brief Assign a value to the entire array. + * @param value Value to be assigned. + * @return Reference to the InterpolatedArray object. + */ + InterpolatedArray & operator=(const T & value) + { + std::fill(values_.begin(), values_.end(), value); + interpolator_->build(bases_, values_); + return *this; + } + + /** + * @brief Compute the interpolated value at a given position. + * @param x The position to compute the value at. + * @return The interpolated value. + */ + T compute(const double & x) const { return interpolator_->compute(x); } + + /** + * @brief Get the underlying data of the array. + * @return A pair containing the axis and values. + */ + std::pair, std::vector> get_data() const { return {bases_, values_}; } +}; + +} // namespace autoware::motion_utils::trajectory_container::trajectory::detail + +// clang-format off +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ // NOLINT +// clang-format on diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/detail/utils.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/detail/utils.hpp new file mode 100644 index 0000000000000..9aca9ba549c2b --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/detail/utils.hpp @@ -0,0 +1,75 @@ +// 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_CONTAINER__TRAJECTORY__DETAIL__UTILS_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__DETAIL__UTILS_HPP_ + +// #include "lanelet2_core/primitives/Point.h" + +#include + +#include +#include +#include +#include + +#include +#include + +namespace autoware::motion_utils::trajectory_container::trajectory::detail +{ +/** + * @brief Convert various point types to geometry_msgs::msg::Point. + * @param p The input point to be converted. + * @return geometry_msgs::msg::Point The converted point. + */ +geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Point & p); +geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Pose & p); +geometry_msgs::msg::Point to_point(const Eigen::Ref & p); +geometry_msgs::msg::Point to_point(const autoware_planning_msgs::msg::PathPoint & p); +geometry_msgs::msg::Point to_point(const tier4_planning_msgs::msg::PathPointWithLaneId & p); +// geometry_msgs::msg::Point to_point(const lanelet::BasicPoint2d & p); +// geometry_msgs::msg::Point to_point(const lanelet::ConstPoint3d & p); + +/** + * @brief Merge multiple vectors into one, keeping only unique elements. + * @tparam Vectors Variadic template parameter for vector types. + * @param vectors Vectors to be merged. + * @return std::vector Merged vector with unique elements. + */ +template +std::vector merge_vectors(const Vectors &... vectors) +{ + std::set unique_elements; + + // Helper function to insert elements into the set + auto insert_elements = [&unique_elements](const auto & vec) { + unique_elements.insert(vec.begin(), vec.end()); + }; + + // Expand the parameter pack and insert elements from each vector + (insert_elements(vectors), ...); + + // Convert the set to std::vector + return {unique_elements.begin(), unique_elements.end()}; +} + +std::vector fill_bases(const std::vector & x, const size_t & min_points); + +std::vector crop_bases( + const std::vector & x, const double & start, const double & end); + +} // namespace autoware::motion_utils::trajectory_container::trajectory::detail + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__DETAIL__UTILS_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point.hpp new file mode 100644 index 0000000000000..3341e38cd87ae --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point.hpp @@ -0,0 +1,146 @@ +// 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_CONTAINER__TRAJECTORY__TRAJECTORY_PATH_POINT_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_PATH_POINT_HPP_ + +#include "autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp" +#include "autoware/motion_utils/trajectory_container/trajectory/detail/interpolated_array.hpp" +#include "autoware/motion_utils/trajectory_container/trajectory/trajectory_pose.hpp" + +// #include +// #include + +#include +#include +#include + +namespace autoware::motion_utils::trajectory_container::trajectory +{ +template <> +class TrajectoryContainer +: public TrajectoryContainer +{ + using BaseClass = TrajectoryContainer; + using PointType = autoware_planning_msgs::msg::PathPoint; + +public: + detail::InterpolatedArray longitudinal_velocity_mps{ + nullptr}; //!< Longitudinal velocity in m/s + detail::InterpolatedArray lateral_velocity_mps{nullptr}; //!< Lateral velocity in m/s + detail::InterpolatedArray heading_rate_rps{nullptr}; //!< Heading rate in rad/s}; + + /** + * @brief Build the trajectory from the points + * @param points Vector of points + * @return True if the build is successful + */ + bool build(const std::vector & points); + + /** + * @brief Compute the point on the trajectory at a given s value + * @param s Arc length + * @return Point on the trajectory + */ + [[nodiscard]] PointType compute(double s) const; + + /** + * @brief Restore the trajectory points + * @param min_points Minimum number of points + * @return Vector of points + */ + [[nodiscard]] std::vector restore(const size_t & min_points = 100) const; + + class Builder + { + private: + std::unique_ptr trajectory_; + + public: + Builder() + { + trajectory_ = std::make_unique(); + set_xy_interpolator(); + set_z_interpolator(); + set_orientation_interpolator(); + set_longitudinal_velocity_interpolator>(); + set_lateral_velocity_interpolator>(); + set_heading_rate_interpolator>(); + } + + template + Builder & set_xy_interpolator(Args &&... args) + { + trajectory_->x_interpolator_ = + std::make_shared(std::forward(args)...); + trajectory_->y_interpolator_ = + std::make_shared(std::forward(args)...); + return *this; + } + + template + Builder & set_z_interpolator(Args &&... args) + { + trajectory_->z_interpolator_ = + std::make_shared(std::forward(args)...); + return *this; + } + + template + Builder & set_orientation_interpolator(Args &&... args) + { + trajectory_->orientation_interpolator_ = + std::make_shared(std::forward(args)...); + return *this; + } + + template + Builder & set_longitudinal_velocity_interpolator(Args &&... args) + { + trajectory_->longitudinal_velocity_mps = detail::InterpolatedArray( + std::make_shared(std::forward(args)...)); + return *this; + } + + template + Builder & set_lateral_velocity_interpolator(Args &&... args) + { + trajectory_->lateral_velocity_mps = detail::InterpolatedArray( + std::make_shared(std::forward(args)...)); + return *this; + } + + template + Builder & set_heading_rate_interpolator(Args &&... args) + { + trajectory_->heading_rate_rps = detail::InterpolatedArray( + std::make_shared(std::forward(args)...)); + return *this; + } + + std::optional build(const std::vector & points) + { + if (trajectory_->build(points)) { + auto result = std::make_optional(std::move(*trajectory_)); + trajectory_.reset(); + return result; + } + return std::nullopt; + } + }; +}; + +} // namespace autoware::motion_utils::trajectory_container::trajectory + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_PATH_POINT_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point_with_lane_id.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point_with_lane_id.hpp new file mode 100644 index 0000000000000..dfad421da4881 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point_with_lane_id.hpp @@ -0,0 +1,154 @@ +// 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. + +// clang-format off +#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_PATH_POINT_WITH_LANE_ID_HPP_ // NOLINT +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_PATH_POINT_WITH_LANE_ID_HPP_ // NOLINT +// clang-format on + +#include "autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point.hpp" +#include "autoware/motion_utils/trajectory_container/trajectory/trajectory_point.hpp" + +#include + +#include +#include +#include + +namespace autoware::motion_utils::trajectory_container::trajectory +{ +template <> +class TrajectoryContainer +: public TrajectoryContainer +{ + using BaseClass = TrajectoryContainer; + using PointType = tier4_planning_msgs::msg::PathPointWithLaneId; + using LaneIdType = std::vector; + +public: + detail::InterpolatedArray lane_ids{nullptr}; //!< Lane ID + + /** + * @brief Build the trajectory from the points + * @param points Vector of points + * @return True if the build is successful + */ + bool build(const std::vector & points); + + /** + * @brief Compute the point on the trajectory at a given s value + * @param s Arc length + * @return Point on the trajectory + */ + [[nodiscard]] PointType compute(double s) const; + + /** + * @brief Restore the trajectory points + * @param min_points Minimum number of points + * @return Vector of points + */ + [[nodiscard]] std::vector restore(const size_t & min_points = 100) const; + + class Builder + { + private: + std::unique_ptr trajectory_; + + public: + Builder() + { + trajectory_ = std::make_unique(); + set_xy_interpolator(); + set_z_interpolator(); + set_orientation_interpolator(); + set_longitudinal_velocity_interpolator>(); + set_lateral_velocity_interpolator>(); + set_heading_rate_interpolator>(); + set_lane_ids_interpolator>(); + } + + template + Builder & set_xy_interpolator(Args &&... args) + { + trajectory_->x_interpolator_ = + std::make_shared(std::forward(args)...); + trajectory_->y_interpolator_ = + std::make_shared(std::forward(args)...); + return *this; + } + + template + Builder & set_z_interpolator(Args &&... args) + { + trajectory_->z_interpolator_ = + std::make_shared(std::forward(args)...); + return *this; + } + + template + Builder & set_orientation_interpolator(Args &&... args) + { + trajectory_->orientation_interpolator_ = + std::make_shared(std::forward(args)...); + return *this; + } + + template + Builder & set_longitudinal_velocity_interpolator(Args &&... args) + { + trajectory_->longitudinal_velocity_mps = detail::InterpolatedArray( + std::make_shared(std::forward(args)...)); + return *this; + } + + template + Builder & set_lateral_velocity_interpolator(Args &&... args) + { + trajectory_->lateral_velocity_mps = detail::InterpolatedArray( + std::make_shared(std::forward(args)...)); + return *this; + } + + template + Builder & set_heading_rate_interpolator(Args &&... args) + { + trajectory_->heading_rate_rps = detail::InterpolatedArray( + std::make_shared(std::forward(args)...)); + return *this; + } + + template + Builder & set_lane_ids_interpolator(Args &&... args) + { + trajectory_->lane_ids = detail::InterpolatedArray( + std::make_shared(std::forward(args)...)); + return *this; + } + + std::optional build(const std::vector & points) + { + if (trajectory_->build(points)) { + auto result = std::make_optional(std::move(*trajectory_)); + trajectory_.reset(); + return result; + } + return std::nullopt; + } + }; +}; +} // namespace autoware::motion_utils::trajectory_container::trajectory + +// clang-format off +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_PATH_POINT_WITH_LANE_ID_HPP_ // NOLINT +// clang-format on diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_point.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_point.hpp new file mode 100644 index 0000000000000..81065924007dc --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_point.hpp @@ -0,0 +1,297 @@ +// 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_CONTAINER__TRAJECTORY__TRAJECTORY_POINT_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_POINT_HPP_ + +#include "autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/linear.hpp" +#include "autoware/motion_utils/trajectory_container/trajectory/detail/utils.hpp" + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::motion_utils::trajectory_container::trajectory +{ + +template +class TrajectoryContainer; + +/** + * @brief Trajectory class for geometry_msgs::msg::Point + */ +template <> +class TrajectoryContainer +{ + using PointType = geometry_msgs::msg::Point; + +protected: + std::shared_ptr> + x_interpolator_; //!< Interpolator for x + std::shared_ptr> + y_interpolator_; //!< Interpolator for y + std::shared_ptr> + z_interpolator_; //!< Interpolator for z + + std::vector bases_; //!< Axis of the trajectory + + double start_{0.0}, end_{0.0}; //!< Start and end of the arc length of the trajectory + + using ConstraintFunction = std::function; + + /** + * @brief Validate the arc length is within the trajectory + * @param s Arc length + */ + [[nodiscard]] double clamp(const double & s, bool show_warning = false) const; + +public: + /** + * @brief Get the length of the trajectory + * @return Length of the trajectory + */ + [[nodiscard]] double length() const; + + /** + * @brief Compute the point on the trajectory at a given s value + * @param s Arc length + * @return Point on the trajectory + */ + [[nodiscard]] PointType compute(double s) const; + + /** + * @brief Build the trajectory from the points + * @param points Vector of points + * @return True if the build is successful + */ + bool build(const std::vector & points); + + /** + * @brief Get the azimuth angle at a given s value + * @param s Arc length + * @return Azimuth in radians + */ + [[nodiscard]] double azimuth(double s) const; + + /** + * @brief Get the elevation angle at a given s value + * @param s Arc length + * @return Elevation in radians + */ + [[nodiscard]] double elevation(double s) const; + + /** + * @brief Get the curvature at a given s value + * @param s Arc length + * @return Curvature + */ + [[nodiscard]] double curvature(double s) const; + + /** + * @brief Find the closest point with constraint + * @tparam InputPointType Type of input point + * @param p Input point + * @param constraints Constraint function + * @return Optional arc length of the closest point + */ + template + [[nodiscard]] std::optional closest_with_constraint( + const InputPointType & p, const ConstraintFunction & constraints) const + { + using motion_utils::trajectory_container::trajectory::detail::to_point; + Eigen::Vector2d point(to_point(p).x, to_point(p).y); + std::vector distances_from_segments; + std::vector lengthes_from_start_points; + + auto axis = detail::crop_bases(bases_, start_, end_); + + for (size_t i = 1; i < axis.size(); ++i) { + Eigen::Vector2d p0; + Eigen::Vector2d p1; + p0 << x_interpolator_->compute(axis.at(i - 1)), y_interpolator_->compute(axis.at(i - 1)); + p1 << x_interpolator_->compute(axis.at(i)), y_interpolator_->compute(axis.at(i)); + Eigen::Vector2d v = p1 - p0; + Eigen::Vector2d w = point - p0; + double c1 = w.dot(v); + double c2 = v.dot(v); + double length_from_start_point = NAN; + double distance_from_segment = NAN; + if (c1 <= 0) { + length_from_start_point = axis.at(i - 1); + distance_from_segment = (point - p0).norm(); + } else if (c2 <= c1) { + length_from_start_point = axis.at(i); + distance_from_segment = (point - p1).norm(); + } else { + length_from_start_point = axis.at(i - 1) + c1 / c2 * (p1 - p0).norm(); + distance_from_segment = (point - (p0 + (c1 / c2) * v)).norm(); + } + if (constraints(length_from_start_point)) { + distances_from_segments.push_back(distance_from_segment); + lengthes_from_start_points.push_back(length_from_start_point); + } + } + if (distances_from_segments.empty()) { + return std::nullopt; + } + + auto min_it = std::min_element(distances_from_segments.begin(), distances_from_segments.end()); + + return lengthes_from_start_points[std::distance(distances_from_segments.begin(), min_it)] - + start_; + } + + /** + * @brief Find the closest point + * @tparam InputPointType Type of input point + * @param p Input point + * @return Arc length of the closest point + */ + template + [[nodiscard]] double closest(const InputPointType & p) const + { + auto s = closest_with_constraint(p, [](const double &) { return true; }); + return *s; + } + /** + * @brief Find the crossing point with constraint + * @tparam InputPointType Type of input point + * @param start Start point + * @param end End point + * @param constraints Constraint function + * @return Optional arc length of the crossing point + */ + template + [[nodiscard]] std::optional crossed_with_constraint( + const InputPointType & start, const InputPointType & end, + const ConstraintFunction & constraints) const + { + using motion_utils::trajectory_container::trajectory::detail::to_point; + Eigen::Vector2d line_start(to_point(start).x, to_point(start).y); + Eigen::Vector2d line_end(to_point(end).x, to_point(end).y); + Eigen::Vector2d line_dir = line_end - line_start; + + auto axis = detail::crop_bases(bases_, start_, end_); + + for (size_t i = 1; i < axis.size(); ++i) { + Eigen::Vector2d p0; + Eigen::Vector2d p1; + p0 << x_interpolator_->compute(axis.at(i - 1)), y_interpolator_->compute(axis.at(i - 1)); + p1 << x_interpolator_->compute(axis.at(i)), y_interpolator_->compute(axis.at(i)); + + Eigen::Vector2d segment_dir = p1 - p0; + + double det = segment_dir.x() * line_dir.y() - segment_dir.y() * line_dir.x(); + + if (std::abs(det) < 1e-10) { + continue; + } + + Eigen::Vector2d p0_to_line_start = line_start - p0; + + double t = (p0_to_line_start.x() * line_dir.y() - p0_to_line_start.y() * line_dir.x()) / det; + double u = + (p0_to_line_start.x() * segment_dir.y() - p0_to_line_start.y() * segment_dir.x()) / det; + + if (t >= 0.0 && t <= 1.0 && u >= 0.0 && u <= 1.0) { + double intersection_s = axis.at(i - 1) + t * (axis.at(i) - axis.at(i - 1)); + if (constraints(intersection_s)) { + return intersection_s - start_; + } + } + } + + return std::nullopt; + } + + /** + * @brief Find the crossing point + * @tparam InputPointType Type of input point + * @param start Start point + * @param end End point + * @return Optional arc length of the crossing point + */ + template + [[nodiscard]] std::optional crossed( + const InputPointType & start, const InputPointType & end) const + { + return crossed_with_constraint(start, end, [](const double &) { return true; }); + } + + /** + * @brief Restore the trajectory points + * @param min_points Minimum number of points + * @return Vector of points + */ + [[nodiscard]] std::vector restore(const size_t & min_points = 100) const; + + void crop(const double & start, const double & length); + + class Builder + { + private: + std::unique_ptr trajectory_; + + public: + Builder() + { + trajectory_ = std::make_unique(); + // Default interpolators + set_xy_interpolator(); + set_z_interpolator(); + } + + template + Builder & set_xy_interpolator(Args &&... args) + { + trajectory_->x_interpolator_ = + std::make_shared(std::forward(args)...); + trajectory_->y_interpolator_ = + std::make_shared(std::forward(args)...); + return *this; + } + + template + Builder & set_z_interpolator(Args &&... args) + { + trajectory_->z_interpolator_ = + std::make_shared(std::forward(args)...); + return *this; + } + + std::optional build(const std::vector & points) + { + if (trajectory_->build(points)) { + auto result = std::make_optional(std::move(*trajectory_)); + trajectory_.reset(); + return result; + } + return std::nullopt; + } + }; +}; + +} // namespace autoware::motion_utils::trajectory_container::trajectory + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_POINT_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_pose.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_pose.hpp new file mode 100644 index 0000000000000..ffebaf290d939 --- /dev/null +++ b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_pose.hpp @@ -0,0 +1,121 @@ +// 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_CONTAINER__TRAJECTORY__TRAJECTORY_POSE_HPP_ +#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_POSE_HPP_ + +#include "autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" +#include "autoware/motion_utils/trajectory_container/interpolator/spherical_linear.hpp" +#include "autoware/motion_utils/trajectory_container/trajectory/trajectory_point.hpp" + +#include + +#include +#include +#include + +namespace autoware::motion_utils::trajectory_container::trajectory +{ + +/** + * @brief Trajectory class for geometry_msgs::msg::Pose + */ +template <> +class TrajectoryContainer +: public TrajectoryContainer +{ + using BaseClass = TrajectoryContainer; + using PointType = geometry_msgs::msg::Pose; + +protected: + std::shared_ptr> + orientation_interpolator_; //!< Interpolator for orientations + +public: + bool build(const std::vector & points); + + /** + * @brief Compute the pose on the trajectory at a given s value + * @param s Arc length + * @return Pose on the trajectory + */ + [[nodiscard]] PointType compute(double s) const; + + /** + * @brief Restore the trajectory poses + * @return Vector of poses + */ + [[nodiscard]] std::vector restore(const size_t & min_points = 100) const; + + /** + * @brief Align the orientation with the direction + */ + void align_orientation_with_trajectory_direction(); + + class Builder + { + private: + std::unique_ptr trajectory_; + + public: + Builder() + { + trajectory_ = std::make_unique(); + set_xy_interpolator(); + set_z_interpolator(); + set_orientation_interpolator(); + } + + template + Builder & set_xy_interpolator(Args &&... args) + { + trajectory_->x_interpolator_ = + std::make_shared(std::forward(args)...); + trajectory_->y_interpolator_ = + std::make_shared(std::forward(args)...); + return *this; + } + + template + Builder & set_z_interpolator(Args &&... args) + { + trajectory_->z_interpolator_ = + std::make_shared(std::forward(args)...); + return *this; + } + + template + Builder & set_orientation_interpolator(Args &&... args) + { + trajectory_->orientation_interpolator_ = + std::make_shared(std::forward(args)...); + return *this; + } + + std::optional build(const std::vector & points) + { + if (trajectory_->build(points)) { + auto result = std::make_optional(std::move(*trajectory_)); + trajectory_.reset(); + return result; + } + return std::nullopt; + } + }; +}; + +} // namespace autoware::motion_utils::trajectory_container::trajectory + +#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_POSE_HPP_ diff --git a/common/autoware_motion_utils/package.xml b/common/autoware_motion_utils/package.xml index 1fe2c4d8e1ea8..c3429022e3a59 100644 --- a/common/autoware_motion_utils/package.xml +++ b/common/autoware_motion_utils/package.xml @@ -34,6 +34,7 @@ tf2_geometry_msgs tier4_planning_msgs visualization_msgs + ament_cmake_ros ament_lint_auto diff --git a/common/autoware_motion_utils/src/trajectory_container/trajectory/detail/util.cpp b/common/autoware_motion_utils/src/trajectory_container/trajectory/detail/util.cpp new file mode 100644 index 0000000000000..9886036604109 --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory_container/trajectory/detail/util.cpp @@ -0,0 +1,132 @@ +// 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_container/trajectory/detail/utils.hpp" + +#include +#include + +namespace autoware::motion_utils::trajectory_container::trajectory::detail +{ + +geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Point & p) +{ + return p; +} + +geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Pose & p) +{ + return p.position; +} + +geometry_msgs::msg::Point to_point(const Eigen::Ref & p) +{ + geometry_msgs::msg::Point point; + point.x = p(0); + point.y = p(1); + return point; +} + +geometry_msgs::msg::Point to_point(const autoware_planning_msgs::msg::PathPoint & p) +{ + geometry_msgs::msg::Point point; + point.x = p.pose.position.x; + point.y = p.pose.position.y; + return point; +} + +geometry_msgs::msg::Point to_point(const tier4_planning_msgs::msg::PathPointWithLaneId & p) +{ + geometry_msgs::msg::Point point; + point.x = p.point.pose.position.x; + point.y = p.point.pose.position.y; + return point; +} + +// geometry_msgs::msg::Point to_point(const lanelet::BasicPoint2d & p) +// { +// geometry_msgs::msg::Point point; +// point.x = p.x(); +// point.y = p.y(); +// return point; +// } + +// geometry_msgs::msg::Point to_point(const lanelet::ConstPoint3d & p) +// { +// geometry_msgs::msg::Point point; +// point.x = p.x(); +// point.y = p.y(); +// return point; +// } + +std::vector fill_bases(const std::vector & x, const size_t & min_points) +{ + const auto original_size = x.size(); + + if (original_size >= min_points) { + return x; + } + + const auto points_to_add = min_points - original_size; + const auto num_gaps = original_size - 1; + + std::vector points_per_gap(num_gaps, points_to_add / num_gaps); + std::fill_n(points_per_gap.begin(), points_to_add % num_gaps, points_per_gap[0] + 1); + + std::vector result; + result.reserve(min_points); + + for (size_t i = 0; i < original_size - 1; ++i) { + result.push_back(x[i]); + + const double start = x[i]; + const double end = x[i + 1]; + const double step = (end - start) / static_cast(points_per_gap[i] + 1); + + for (size_t j = 0; j < points_per_gap[i]; ++j) { + result.push_back(start + static_cast(j + 1) * step); + } + } + + result.push_back(x[original_size - 1]); + + return result; +} + +std::vector crop_bases( + const std::vector & x, const double & start, const double & end) +{ + std::vector result; + + // Add start point if it's not already in x + if (std::find(x.begin(), x.end(), start) == x.end()) { + result.push_back(start); + } + + // Copy all points within the range [start, end] + for (double i : x) { + if (i >= start && i <= end) { + result.push_back(i); + } + } + + // Add end point if it's not already in x + if (std::find(x.begin(), x.end(), end) == x.end()) { + result.push_back(end); + } + + return result; +} + +} // namespace autoware::motion_utils::trajectory_container::trajectory::detail diff --git a/common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_path_point.cpp b/common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_path_point.cpp new file mode 100644 index 0000000000000..8979e17b5b9e1 --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_path_point.cpp @@ -0,0 +1,89 @@ +// 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_container/trajectory/trajectory_path_point.hpp" + +#include "autoware/motion_utils/trajectory_container/trajectory/detail/utils.hpp" + +#include + +namespace autoware::motion_utils::trajectory_container::trajectory +{ + +using PointType = autoware_planning_msgs::msg::PathPoint; + +bool TrajectoryContainer::build(const std::vector & points) +{ + std::vector poses; + std::vector longitudinal_velocity_mps_values; + std::vector lateral_velocity_mps_values; + std::vector heading_rate_rps_values; + + for (const auto & point : points) { + poses.emplace_back(point.pose); + longitudinal_velocity_mps_values.emplace_back(point.longitudinal_velocity_mps); + lateral_velocity_mps_values.emplace_back(point.lateral_velocity_mps); + heading_rate_rps_values.emplace_back(point.heading_rate_rps); + } + + bool is_valid = true; + + is_valid &= TrajectoryContainer::build(poses); + is_valid &= this->longitudinal_velocity_mps.build(bases_, longitudinal_velocity_mps_values); + is_valid &= this->lateral_velocity_mps.build(bases_, lateral_velocity_mps_values); + is_valid &= this->heading_rate_rps.build(bases_, heading_rate_rps_values); + + return is_valid; +} + +PointType TrajectoryContainer::compute(double s) const +{ + PointType result; + result.pose = TrajectoryContainer::compute(s); + s = clamp(s); + result.longitudinal_velocity_mps = static_cast(this->longitudinal_velocity_mps.compute(s)); + result.lateral_velocity_mps = static_cast(this->lateral_velocity_mps.compute(s)); + result.heading_rate_rps = static_cast(this->heading_rate_rps.compute(s)); + return result; +} + +std::vector TrajectoryContainer::restore(const size_t & min_points) const +{ + auto get_bases = [](const auto & interpolated_array) { + auto [bases, values] = interpolated_array.get_data(); + return bases; + }; + + auto bases = detail::merge_vectors( + bases_, get_bases(this->longitudinal_velocity_mps), get_bases(this->lateral_velocity_mps), + get_bases(this->heading_rate_rps)); + + bases = detail::crop_bases(bases, start_, end_); + bases = detail::fill_bases(bases, min_points); + + std::vector points; + points.reserve(bases.size()); + for (const auto & s : bases) { + PointType p; + p.pose = TrajectoryContainer::compute(s); + p.longitudinal_velocity_mps = static_cast(this->longitudinal_velocity_mps.compute(s)); + p.lateral_velocity_mps = static_cast(this->lateral_velocity_mps.compute(s)); + p.heading_rate_rps = static_cast(this->heading_rate_rps.compute(s)); + points.emplace_back(p); + } + + return points; +} + +} // namespace autoware::motion_utils::trajectory_container::trajectory diff --git a/common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_path_point_with_lane_id.cpp b/common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_path_point_with_lane_id.cpp new file mode 100644 index 0000000000000..7bfe6ec788db1 --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_path_point_with_lane_id.cpp @@ -0,0 +1,73 @@ +// 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_container/trajectory/trajectory_path_point_with_lane_id.hpp" + +#include "autoware/motion_utils/trajectory_container/trajectory/detail/utils.hpp" + +namespace autoware::motion_utils::trajectory_container::trajectory +{ + +using PointType = tier4_planning_msgs::msg::PathPointWithLaneId; + +bool TrajectoryContainer::build(const std::vector & points) +{ + std::vector path_points; + std::vector> lane_ids_values; + + for (const auto & point : points) { + path_points.emplace_back(point.point); + lane_ids_values.emplace_back(point.lane_ids); + } + bool is_valid = true; + is_valid &= BaseClass::build(path_points); + is_valid &= lane_ids.build(bases_, lane_ids_values); + return is_valid; +} + +PointType TrajectoryContainer::compute(double s) const +{ + PointType result; + result.point = BaseClass::compute(s); + s = clamp(s); + result.lane_ids = lane_ids.compute(s); + return result; +} + +std::vector TrajectoryContainer::restore(const size_t & min_points) const +{ + auto get_bases = [](const auto & interpolated_array) { + auto [bases, values] = interpolated_array.get_data(); + return bases; + }; + + auto bases = detail::merge_vectors( + bases_, get_bases(this->longitudinal_velocity_mps), get_bases(this->lateral_velocity_mps), + get_bases(this->heading_rate_rps), get_bases(this->lane_ids)); + + bases = detail::crop_bases(bases, start_, end_); + bases = detail::fill_bases(bases, min_points); + + std::vector points; + points.reserve(bases.size()); + for (const auto & s : bases) { + PointType p; + p.point = BaseClass::compute(s); + p.lane_ids = lane_ids.compute(s); + points.emplace_back(p); + } + return points; +} + +} // namespace autoware::motion_utils::trajectory_container::trajectory diff --git a/common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_point.cpp b/common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_point.cpp new file mode 100644 index 0000000000000..324853024466a --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_point.cpp @@ -0,0 +1,134 @@ +// 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_container/trajectory/trajectory_point.hpp" + +#include "autoware/motion_utils/trajectory_container/trajectory/detail/utils.hpp" + +#include +#include + +#include +#include +#include + +namespace autoware::motion_utils::trajectory_container::trajectory +{ + +using PointType = geometry_msgs::msg::Point; + +bool TrajectoryContainer::build(const std::vector & points) +{ + std::vector xs; + std::vector ys; + std::vector zs; + + bases_.emplace_back(0.0); + xs.emplace_back(points[0].x); + ys.emplace_back(points[0].y); + zs.emplace_back(points[0].z); + + for (size_t i = 1; i < points.size(); ++i) { + Eigen::Vector2d p0(points[i - 1].x, points[i - 1].y); + Eigen::Vector2d p1(points[i].x, points[i].y); + bases_.emplace_back(bases_.back() + (p1 - p0).norm()); + xs.emplace_back(points[i].x); + ys.emplace_back(points[i].y); + zs.emplace_back(points[i].z); + } + + start_ = bases_.front(); + end_ = bases_.back(); + + bool is_valid = true; + is_valid &= x_interpolator_->build(bases_, xs); + is_valid &= y_interpolator_->build(bases_, ys); + is_valid &= z_interpolator_->build(bases_, zs); + + return is_valid; +} + +double TrajectoryContainer::clamp(const double & s, bool show_warning) const +{ + if ((s < 0 || s > length()) && show_warning) { + RCLCPP_WARN( + rclcpp::get_logger("TrajectoryContainer"), + "The arc length %f is out of the trajectory length %f", s, length()); + } + return std::clamp(s, 0.0, length()) + start_; +} + +double TrajectoryContainer::length() const +{ + return end_ - start_; +} + +PointType TrajectoryContainer::compute(double s) const +{ + s = clamp(s, true); + PointType result; + result.x = x_interpolator_->compute(s); + result.y = y_interpolator_->compute(s); + result.z = z_interpolator_->compute(s); + return result; +} + +double TrajectoryContainer::azimuth(double s) const +{ + s = clamp(s, true); + double dx = x_interpolator_->compute_first_derivative(s); + double dy = y_interpolator_->compute_first_derivative(s); + return std::atan2(dy, dx); +} + +double TrajectoryContainer::elevation(double s) const +{ + s = clamp(s, true); + double dz = z_interpolator_->compute_first_derivative(s); + return std::atan2(dz, 1.0); +} + +double TrajectoryContainer::curvature(double s) const +{ + s = clamp(s, true); + double dx = x_interpolator_->compute_first_derivative(s); + double ddx = x_interpolator_->compute_second_derivative(s); + double dy = y_interpolator_->compute_first_derivative(s); + double ddy = y_interpolator_->compute_second_derivative(s); + return std::abs(dx * ddy - dy * ddx) / std::pow(dx * dx + dy * dy, 1.5); +} + +std::vector TrajectoryContainer::restore(const size_t & min_points) const +{ + auto bases = detail::crop_bases(bases_, start_, end_); + bases = detail::fill_bases(bases, static_cast(min_points)); + std::vector points; + points.reserve(bases.size()); + for (const auto & s : bases) { + PointType p; + p.x = x_interpolator_->compute(s); + p.y = y_interpolator_->compute(s); + p.z = z_interpolator_->compute(s); + points.emplace_back(p); + } + return points; +} + +void TrajectoryContainer::crop(const double & start, const double & length) +{ + start_ = std::clamp(start_ + start, start_, end_); + end_ = std::clamp(start_ + length, start_, end_); +} + +} // namespace autoware::motion_utils::trajectory_container::trajectory diff --git a/common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_pose.cpp b/common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_pose.cpp new file mode 100644 index 0000000000000..0c02f09e8ac3c --- /dev/null +++ b/common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_pose.cpp @@ -0,0 +1,102 @@ +// 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_container/trajectory/trajectory_pose.hpp" + +#include +#include + +#include + +namespace autoware::motion_utils::trajectory_container::trajectory +{ + +using PointType = geometry_msgs::msg::Pose; + +bool TrajectoryContainer::build(const std::vector & points) +{ + std::vector path_points; + std::vector orientations; + path_points.reserve(points.size()); + orientations.reserve(points.size()); + for (const auto & point : points) { + path_points.emplace_back(point.position); + orientations.emplace_back(point.orientation); + } + + bool is_valid = true; + is_valid &= BaseClass::build(path_points); + is_valid &= orientation_interpolator_->build(bases_, orientations); + return is_valid; +} + +PointType TrajectoryContainer::compute(double s) const +{ + PointType result; + result.position = BaseClass::compute(s); + result.orientation = orientation_interpolator_->compute(s); + s = clamp(s); + return result; +} + +void TrajectoryContainer::align_orientation_with_trajectory_direction() +{ + std::vector aligned_orientations; + for (const auto & s : bases_) { + double azimuth = this->azimuth(s); + double elevation = this->elevation(s); + geometry_msgs::msg::Quaternion current_orientation = orientation_interpolator_->compute(s); + tf2::Quaternion current_orientation_tf2( + current_orientation.x, current_orientation.y, current_orientation.z, current_orientation.w); + current_orientation_tf2.normalize(); + tf2::Vector3 x_axis(1.0, 0.0, 0.0); + tf2::Vector3 current_x_axis = tf2::quatRotate(current_orientation_tf2, x_axis); + + tf2::Vector3 desired_x_axis( + std::cos(elevation) * std::cos(azimuth), std::cos(elevation) * std::sin(azimuth), + std::sin(elevation)); + tf2::Vector3 rotation_axis = current_x_axis.cross(desired_x_axis).normalized(); + double dot_product = current_x_axis.dot(desired_x_axis); + double rotation_angle = std::acos(dot_product); + + tf2::Quaternion delta_q(rotation_axis, rotation_angle); + tf2::Quaternion aligned_orientation_tf2 = (delta_q * current_orientation_tf2).normalized(); + + geometry_msgs::msg::Quaternion aligned_orientation; + aligned_orientation.x = aligned_orientation_tf2.x(); + aligned_orientation.y = aligned_orientation_tf2.y(); + aligned_orientation.z = aligned_orientation_tf2.z(); + aligned_orientation.w = aligned_orientation_tf2.w(); + + aligned_orientations.emplace_back(aligned_orientation); + } + orientation_interpolator_->build(bases_, aligned_orientations); +} + +std::vector TrajectoryContainer::restore(const size_t & min_points) const +{ + auto bases = detail::crop_bases(bases_, start_, end_); + bases = detail::fill_bases(bases, static_cast(min_points)); + std::vector points; + points.reserve(bases.size()); + for (const auto & s : bases) { + PointType p; + p.position = BaseClass::compute(s); + p.orientation = orientation_interpolator_->compute(s); + points.emplace_back(p); + } + return points; +} + +} // namespace autoware::motion_utils::trajectory_container::trajectory diff --git a/common/autoware_motion_utils/test/src/trajectory_container/test_trajectory_container.cpp b/common/autoware_motion_utils/test/src/trajectory_container/test_trajectory_container.cpp new file mode 100644 index 0000000000000..e5c054ab9abf7 --- /dev/null +++ b/common/autoware_motion_utils/test/src/trajectory_container/test_trajectory_container.cpp @@ -0,0 +1,198 @@ +// 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 + +using Trajectory = autoware::motion_utils::trajectory_container::trajectory::TrajectoryContainer< + tier4_planning_msgs::msg::PathPointWithLaneId>; + +tier4_planning_msgs::msg::PathPointWithLaneId path_point_with_lane_id( + double x, double y, uint8_t lane_id) +{ + tier4_planning_msgs::msg::PathPointWithLaneId point; + point.point.pose.position.x = x; + point.point.pose.position.y = y; + point.lane_ids.emplace_back(lane_id); + return point; +} + +TEST(TrajectoryCreatorTest, create) +{ + { + std::vector points{ + path_point_with_lane_id(0.00, 0.00, 0)}; + auto trajectory = Trajectory::Builder{}.build(points); + ASSERT_TRUE(!trajectory); + } + { + std::vector points{ + path_point_with_lane_id(0.00, 0.00, 0), path_point_with_lane_id(0.81, 1.68, 0), + path_point_with_lane_id(1.65, 2.98, 0), path_point_with_lane_id(3.30, 4.01, 1)}; + auto trajectory = Trajectory::Builder{}.build(points); + ASSERT_TRUE(trajectory); + } +} + +class TrajectoryContainerTest : public ::testing::Test +{ +public: + std::optional trajectory; + + void SetUp() override + { + std::vector points{ + path_point_with_lane_id(0.00, 0.00, 0), path_point_with_lane_id(0.81, 1.68, 0), + path_point_with_lane_id(1.65, 2.98, 0), path_point_with_lane_id(3.30, 4.01, 1), + path_point_with_lane_id(4.70, 4.52, 1), path_point_with_lane_id(6.49, 5.20, 1), + path_point_with_lane_id(8.11, 6.07, 1), path_point_with_lane_id(8.76, 7.23, 1), + path_point_with_lane_id(9.36, 8.74, 1), path_point_with_lane_id(10.0, 10.0, 1)}; + + trajectory = Trajectory::Builder{}.build(points); + ASSERT_TRUE(trajectory); + } +}; + +TEST_F(TrajectoryContainerTest, compute) +{ + double length = trajectory->length(); + + trajectory->longitudinal_velocity_mps(trajectory->length() / 3.0, trajectory->length()) = 10.0; + + auto point = trajectory->compute(length / 2.0); + + EXPECT_LT(0, point.point.pose.position.x); + EXPECT_LT(point.point.pose.position.x, 10); + + EXPECT_LT(0, point.point.pose.position.y); + EXPECT_LT(point.point.pose.position.y, 10); + + EXPECT_EQ(1, point.lane_ids[0]); +} + +TEST_F(TrajectoryContainerTest, manipulate_velocity) +{ + trajectory->longitudinal_velocity_mps = 10.0; + trajectory->longitudinal_velocity_mps(trajectory->length() / 3, 2.0 * trajectory->length() / 3) = + 5.0; + auto point1 = trajectory->compute(0.0); + auto point2 = trajectory->compute(trajectory->length() / 2.0); + auto point3 = trajectory->compute(trajectory->length()); + + EXPECT_EQ(10.0, point1.point.longitudinal_velocity_mps); + EXPECT_EQ(5.0, point2.point.longitudinal_velocity_mps); + EXPECT_EQ(10.0, point3.point.longitudinal_velocity_mps); +} + +TEST_F(TrajectoryContainerTest, direction) +{ + double dir = trajectory->azimuth(0.0); + EXPECT_LT(0, dir); + EXPECT_LT(dir, M_PI / 2); +} + +TEST_F(TrajectoryContainerTest, curvature) +{ + double curv = trajectory->curvature(0.0); + EXPECT_LT(-1.0, curv); + EXPECT_LT(curv, 1.0); +} + +TEST_F(TrajectoryContainerTest, restore) +{ + using namespace autoware::motion_utils::trajectory_container::trajectory; // NOLINT + trajectory->longitudinal_velocity_mps(4.0, trajectory->length()) = 5.0; + + { + auto points = + static_cast &>(*trajectory).restore(0); + EXPECT_EQ(10, points.size()); + } + + { + auto points = + static_cast &>(*trajectory).restore(0); + EXPECT_EQ(10, points.size()); + } + + { + auto points = + static_cast &>(*trajectory) + .restore(0); + EXPECT_EQ(11, points.size()); + } + + { + auto points = trajectory->restore(0); + EXPECT_EQ(11, points.size()); + } +} + +TEST_F(TrajectoryContainerTest, crossed) +{ + geometry_msgs::msg::Pose pose1; + pose1.position.x = 0.0; + pose1.position.y = 10.0; + geometry_msgs::msg::Pose pose2; + pose2.position.x = 10.0; + pose2.position.y = 0.0; + + auto crossed_point = trajectory->crossed(pose1, pose2); + EXPECT_TRUE(crossed_point.has_value()); + + EXPECT_LT(0.0, *crossed_point); + EXPECT_LT(*crossed_point, trajectory->length()); +} + +TEST_F(TrajectoryContainerTest, closest) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = 5.0; + pose.position.y = 5.0; + + std::cerr << "Closest: " << trajectory->closest(pose) << std::endl; + + auto closest_pose = trajectory->compute(trajectory->closest(pose)); + + double distance = std::hypot( + closest_pose.point.pose.position.x - pose.position.x, + closest_pose.point.pose.position.y - pose.position.y); + + EXPECT_LT(distance, 3.0); +} + +TEST_F(TrajectoryContainerTest, crop) +{ + double length = trajectory->length(); + + auto start_point_expect = trajectory->compute(length / 3.0); + auto end_point_expect = trajectory->compute(length / 3.0 + 1.0); + + trajectory->crop(length / 3.0, 1.0); + + EXPECT_EQ(trajectory->length(), 1.0); + + auto start_point_actual = trajectory->compute(0.0); + auto end_point_actual = trajectory->compute(trajectory->length()); + + EXPECT_EQ(start_point_expect.point.pose.position.x, start_point_actual.point.pose.position.x); + EXPECT_EQ(start_point_expect.point.pose.position.y, start_point_actual.point.pose.position.y); + EXPECT_EQ(start_point_expect.lane_ids[0], start_point_actual.lane_ids[0]); + + EXPECT_EQ(end_point_expect.point.pose.position.x, end_point_actual.point.pose.position.x); + EXPECT_EQ(end_point_expect.point.pose.position.y, end_point_actual.point.pose.position.y); + EXPECT_EQ(end_point_expect.lane_ids[0], end_point_actual.lane_ids[0]); +}