From b6afbf828443ce6badda027d80a87e534c723952 Mon Sep 17 00:00:00 2001 From: Junya Sasaki Date: Sat, 15 Feb 2025 02:21:01 +0900 Subject: [PATCH] feat(autoware_trajectory)!: port to `autoware.core/common` (#10113) porting(autoware_trajectory): port to `autoware.core/common` * Related PR: https://github.com/autowarefoundation/autoware.core/pull/186 Signed-off-by: Junya Sasaki --- common/autoware_trajectory/CHANGELOG.rst | 92 ------ common/autoware_trajectory/CMakeLists.txt | 50 ---- common/autoware_trajectory/README.md | 64 ---- .../examples/example_find_intervals.cpp | 109 ------- .../examples/example_interpolator.cpp | 115 ------- .../examples/example_shift.cpp | 187 ------------ .../example_trajectory_path_point.cpp | 118 -------- .../examples/example_trajectory_point.cpp | 140 --------- .../examples/example_trajectory_pose.cpp | 92 ------ .../autoware/trajectory/detail/helpers.hpp | 72 ----- .../trajectory/detail/interpolated_array.hpp | 217 -------------- .../autoware/trajectory/detail/logging.hpp | 30 -- .../autoware/trajectory/detail/types.hpp | 77 ----- .../include/autoware/trajectory/forward.hpp | 26 -- .../trajectory/interpolator/akima_spline.hpp | 94 ------ .../trajectory/interpolator/cubic_spline.hpp | 96 ------ .../detail/interpolator_common_interface.hpp | 176 ----------- .../detail/interpolator_mixin.hpp | 86 ------ .../detail/nearest_neighbor_common_impl.hpp | 82 ----- .../detail/stairstep_common_impl.hpp | 81 ----- .../trajectory/interpolator/interpolator.hpp | 97 ------ .../trajectory/interpolator/linear.hpp | 86 ------ .../interpolator/nearest_neighbor.hpp | 78 ----- .../interpolator/spherical_linear.hpp | 73 ----- .../trajectory/interpolator/stairstep.hpp | 78 ----- .../autoware/trajectory/path_point.hpp | 170 ----------- .../trajectory/path_point_with_lane_id.hpp | 155 ---------- .../include/autoware/trajectory/point.hpp | 162 ---------- .../include/autoware/trajectory/pose.hpp | 126 -------- .../autoware/trajectory/utils/closest.hpp | 100 ------- .../autoware/trajectory/utils/crop.hpp | 33 --- .../autoware/trajectory/utils/crossed.hpp | 118 -------- .../trajectory/utils/find_intervals.hpp | 76 ----- .../autoware/trajectory/utils/shift.hpp | 119 -------- common/autoware_trajectory/package.xml | 31 -- .../src/detail/logging.cpp | 32 -- .../autoware_trajectory/src/detail/types.cpp | 68 ----- .../autoware_trajectory/src/detail/util.cpp | 83 ------ .../src/interpolator/akima_spline.cpp | 92 ------ .../src/interpolator/cubic_spline.cpp | 93 ------ .../src/interpolator/linear.cpp | 60 ---- .../src/interpolator/spherical_linear.cpp | 64 ---- common/autoware_trajectory/src/path_point.cpp | 130 -------- .../src/path_point_with_lane_id.cpp | 98 ------ common/autoware_trajectory/src/point.cpp | 173 ----------- common/autoware_trajectory/src/pose.cpp | 134 --------- .../autoware_trajectory/src/utils/closest.cpp | 64 ---- .../autoware_trajectory/src/utils/crossed.cpp | 86 ------ .../src/utils/find_intervals.cpp | 42 --- .../autoware_trajectory/src/utils/shift.cpp | 280 ------------------ .../autoware_trajectory/test/test_helpers.cpp | 55 ---- .../test/test_interpolator.cpp | 121 -------- .../test/test_trajectory_container.cpp | 189 ------------ 53 files changed, 5370 deletions(-) delete mode 100644 common/autoware_trajectory/CHANGELOG.rst delete mode 100644 common/autoware_trajectory/CMakeLists.txt delete mode 100644 common/autoware_trajectory/README.md delete mode 100644 common/autoware_trajectory/examples/example_find_intervals.cpp delete mode 100644 common/autoware_trajectory/examples/example_interpolator.cpp delete mode 100644 common/autoware_trajectory/examples/example_shift.cpp delete mode 100644 common/autoware_trajectory/examples/example_trajectory_path_point.cpp delete mode 100644 common/autoware_trajectory/examples/example_trajectory_point.cpp delete mode 100644 common/autoware_trajectory/examples/example_trajectory_pose.cpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/detail/helpers.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/detail/logging.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/forward.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/interpolator/akima_spline.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/interpolator/cubic_spline.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_common_interface.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_mixin.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/nearest_neighbor_common_impl.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/stairstep_common_impl.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/interpolator/linear.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/interpolator/nearest_neighbor.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/interpolator/spherical_linear.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/interpolator/stairstep.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/path_point.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/point.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/pose.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/utils/closest.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/utils/crop.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/utils/crossed.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/utils/find_intervals.hpp delete mode 100644 common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp delete mode 100644 common/autoware_trajectory/package.xml delete mode 100644 common/autoware_trajectory/src/detail/logging.cpp delete mode 100644 common/autoware_trajectory/src/detail/types.cpp delete mode 100644 common/autoware_trajectory/src/detail/util.cpp delete mode 100644 common/autoware_trajectory/src/interpolator/akima_spline.cpp delete mode 100644 common/autoware_trajectory/src/interpolator/cubic_spline.cpp delete mode 100644 common/autoware_trajectory/src/interpolator/linear.cpp delete mode 100644 common/autoware_trajectory/src/interpolator/spherical_linear.cpp delete mode 100644 common/autoware_trajectory/src/path_point.cpp delete mode 100644 common/autoware_trajectory/src/path_point_with_lane_id.cpp delete mode 100644 common/autoware_trajectory/src/point.cpp delete mode 100644 common/autoware_trajectory/src/pose.cpp delete mode 100644 common/autoware_trajectory/src/utils/closest.cpp delete mode 100644 common/autoware_trajectory/src/utils/crossed.cpp delete mode 100644 common/autoware_trajectory/src/utils/find_intervals.cpp delete mode 100644 common/autoware_trajectory/src/utils/shift.cpp delete mode 100644 common/autoware_trajectory/test/test_helpers.cpp delete mode 100644 common/autoware_trajectory/test/test_interpolator.cpp delete mode 100644 common/autoware_trajectory/test/test_trajectory_container.cpp diff --git a/common/autoware_trajectory/CHANGELOG.rst b/common/autoware_trajectory/CHANGELOG.rst deleted file mode 100644 index ba5c731b0b4ce..0000000000000 --- a/common/autoware_trajectory/CHANGELOG.rst +++ /dev/null @@ -1,92 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_trajectory -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.41.0 (2025-01-29) -------------------- - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - common (`#9564 `_) -* refactor: correct spelling (`#9528 `_) -* fix(autoware_trajectory): autoware_trajectory bug (`#9475 `_) - fix autoware_trajectory bug -* 0.39.0 -* fix version -* update changelog -* add changelog -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* refactor(autoware_trajectory): refactor autoware_trajectory (`#9356 `_) -* feat(behavior_velocity_stop_line): replace autoware_motion_utils to autoware_trajectory (`#9299 `_) - * feat(behavior_velocity_stop_line): replace autoware_motion_utils to autoware_trajectory - * update - --------- -* feat(autoware_trajectory): change default value of min_points (`#9315 `_) -* fix(autoware_trajectory): fix bug of autoware_trajectory (`#9314 `_) -* feat(autoware_trajectory): change interface of InterpolatedArray (`#9264 `_) - change interface of InterpolateArray -* feat(autoware_trajectory): move trajectory_container from autoware_motion_utils to a new package (`#9253 `_) - * create trajectory container package - * update - * update - * style(pre-commit): autofix - * update codeowner - * update - * fix cmake - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* Contributors: Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Yukinari Hisaki, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* add changelog -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* feat(autoware_trajectory): change default value of min_points (`#9315 `_) -* fix(autoware_trajectory): fix bug of autoware_trajectory (`#9314 `_) -* feat(autoware_trajectory): change interface of InterpolatedArray (`#9264 `_) - change interface of InterpolateArray -* feat(autoware_trajectory): move trajectory_container from autoware_motion_utils to a new package (`#9253 `_) - * create trajectory container package - * update - * update - * style(pre-commit): autofix - * update codeowner - * update - * fix cmake - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* Contributors: Yukinari Hisaki, Yutaka Kondo - -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* feat(autoware_trajectory): change default value of min_points (`#9315 `_) -* fix(autoware_trajectory): fix bug of autoware_trajectory (`#9314 `_) -* feat(autoware_trajectory): change interface of InterpolatedArray (`#9264 `_) - change interface of InterpolateArray -* feat(autoware_trajectory): move trajectory_container from autoware_motion_utils to a new package (`#9253 `_) - * create trajectory container package - * update - * update - * style(pre-commit): autofix - * update codeowner - * update - * fix cmake - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* Contributors: Yukinari Hisaki, Yutaka Kondo - -0.38.0 (2024-11-11) -------------------- - -0.26.0 (2024-04-03) -------------------- diff --git a/common/autoware_trajectory/CMakeLists.txt b/common/autoware_trajectory/CMakeLists.txt deleted file mode 100644 index 9ce9ddf79bf14..0000000000000 --- a/common/autoware_trajectory/CMakeLists.txt +++ /dev/null @@ -1,50 +0,0 @@ -cmake_minimum_required(VERSION 3.14) - -project(autoware_trajectory) - -option(BUILD_EXAMPLES "Build examples" OFF) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(autoware_trajectory 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_trajectory ${test_files}) - - target_link_libraries(test_autoware_trajectory - autoware_trajectory - ) -endif() - -if(BUILD_EXAMPLES) - message(STATUS "Building examples") - - include(FetchContent) - fetchcontent_declare( - matplotlibcpp17 - GIT_REPOSITORY https://github.com/soblin/matplotlibcpp17.git - GIT_TAG master - ) - fetchcontent_makeavailable(matplotlibcpp17) - - file(GLOB_RECURSE example_files examples/*.cpp) - - foreach(example_file ${example_files}) - get_filename_component(example_name ${example_file} NAME_WE) - ament_auto_add_executable(${example_name} ${example_file}) - set_source_files_properties(${example_file} PROPERTIES COMPILE_FLAGS -Wno-error -Wno-attributes -Wno-unused-parameter) - target_link_libraries(${example_name} - autoware_trajectory - matplotlibcpp17::matplotlibcpp17 - ) - endforeach() -endif() - -ament_auto_package() diff --git a/common/autoware_trajectory/README.md b/common/autoware_trajectory/README.md deleted file mode 100644 index bddeef3b96eae..0000000000000 --- a/common/autoware_trajectory/README.md +++ /dev/null @@ -1,64 +0,0 @@ -# Autoware Trajectory - -This package provides classes to manage/manipulate Trajectory. - -## Example Usage - -This section describes Example Usage of `Trajectory` - -- Load Trajectory from point array - - ```cpp - #include "autoware/trajectory/path_point.hpp" - - ... - - std::vector points = ... // Load points from somewhere - - using autoware::trajectory::Trajectory; - - std::optional> trajectory = - Trajectory::Builder{} - .build(points); - ``` - -- You can also specify interpolation method - - ```cpp - using autoware::trajectory::interpolator::CubicSpline; - - std::optional> trajectory = - Trajectory::Builder{} - .set_xy_interpolator() // Set interpolator for x-y plane - .build(points); - ``` - -- Access point on Trajectory - - ```cpp - autoware_planning_msgs::msg::PathPoint point = trajectory->compute(1.0); // Get point at s=0.0. s is distance from start point on Trajectory. - ``` - -- Get length of Trajectory - - ```cpp - double length = trajectory->length(); - ``` - -- Set 3.0[m] ~ 5.0[m] part of velocity to 0.0 - - ```cpp - trajectory->longitudinal_velocity_mps(3.0, 5.0) = 0.0; - ``` - -- Crop Trajectory from 1.0[m] to 2.0[m] - - ```cpp - trajectory->crop(1.0, 2.0); - ``` - -- Restore points - - ```cpp - std::vector points = trajectory->restore(); - ``` diff --git a/common/autoware_trajectory/examples/example_find_intervals.cpp b/common/autoware_trajectory/examples/example_find_intervals.cpp deleted file mode 100644 index 69d0ed2a38350..0000000000000 --- a/common/autoware_trajectory/examples/example_find_intervals.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright 2025 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/trajectory/path_point_with_lane_id.hpp" -#include "autoware/trajectory/utils/find_intervals.hpp" - -#include - -#include - -using Trajectory = - autoware::trajectory::Trajectory; - -autoware_internal::msg::PathPointWithLaneId path_point_with_lane_id( - double x, double y, uint8_t lane_id) -{ - autoware_internal::msg::PathPointWithLaneId point; - point.point.pose.position.x = x; - point.point.pose.position.y = y; - point.lane_ids.emplace_back(lane_id); - return point; -} - -int main() -{ - pybind11::scoped_interpreter guard{}; - auto plt = matplotlibcpp17::pyplot::import(); - - std::vector points{ - path_point_with_lane_id(0.41, 0.69, 0), path_point_with_lane_id(0.66, 1.09, 0), - path_point_with_lane_id(0.93, 1.41, 0), path_point_with_lane_id(1.26, 1.71, 0), - path_point_with_lane_id(1.62, 1.90, 0), path_point_with_lane_id(1.96, 1.98, 0), - path_point_with_lane_id(2.48, 1.96, 1), path_point_with_lane_id(3.02, 1.87, 1), - path_point_with_lane_id(3.56, 1.82, 1), path_point_with_lane_id(4.14, 2.02, 1), - path_point_with_lane_id(4.56, 2.36, 1), path_point_with_lane_id(4.89, 2.72, 1), - path_point_with_lane_id(5.27, 3.15, 1), path_point_with_lane_id(5.71, 3.69, 1), - path_point_with_lane_id(6.09, 4.02, 0), path_point_with_lane_id(6.54, 4.16, 0), - path_point_with_lane_id(6.79, 3.92, 0), path_point_with_lane_id(7.11, 3.60, 0), - path_point_with_lane_id(7.42, 3.01, 0)}; - - auto trajectory = Trajectory::Builder{}.build(points); - - if (!trajectory) { - return 1; - } - - const auto intervals = autoware::trajectory::find_intervals( - *trajectory, [](const autoware_internal::msg::PathPointWithLaneId & point) { - return point.lane_ids[0] == 1; - }); - - std::vector x_id0; - std::vector y_id0; - std::vector x_id1; - std::vector y_id1; - std::vector x_all; - std::vector y_all; - - for (const auto & point : points) { - if (point.lane_ids[0] == 0) { - x_id0.push_back(point.point.pose.position.x); - y_id0.push_back(point.point.pose.position.y); - } else { - x_id1.push_back(point.point.pose.position.x); - y_id1.push_back(point.point.pose.position.y); - } - } - - for (double s = 0.0; s < trajectory->length(); s += 0.01) { - auto p = trajectory->compute(s); - x_all.push_back(p.point.pose.position.x); - y_all.push_back(p.point.pose.position.y); - } - - plt.plot(Args(x_all, y_all), Kwargs("color"_a = "blue")); - plt.scatter(Args(x_id0, y_id0), Kwargs("color"_a = "blue", "label"_a = "lane_id = 0")); - plt.scatter(Args(x_id1, y_id1), Kwargs("color"_a = "red", "label"_a = "lane_id = 1")); - - std::vector x_cropped; - std::vector y_cropped; - - trajectory->crop(intervals[0].start, intervals[0].end - intervals[0].start); - - for (double s = 0.0; s < trajectory->length(); s += 0.01) { - auto p = trajectory->compute(s); - x_cropped.push_back(p.point.pose.position.x); - y_cropped.push_back(p.point.pose.position.y); - } - - plt.plot( - Args(x_cropped, y_cropped), - Kwargs("color"_a = "red", "label"_a = "interval between lane_id = 1")); - - plt.legend(); - plt.show(); - - return 0; -} diff --git a/common/autoware_trajectory/examples/example_interpolator.cpp b/common/autoware_trajectory/examples/example_interpolator.cpp deleted file mode 100644 index f34d291377884..0000000000000 --- a/common/autoware_trajectory/examples/example_interpolator.cpp +++ /dev/null @@ -1,115 +0,0 @@ -// 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/trajectory/interpolator/akima_spline.hpp" -#include "autoware/trajectory/interpolator/cubic_spline.hpp" -#include "autoware/trajectory/interpolator/interpolator.hpp" -#include "autoware/trajectory/interpolator/linear.hpp" -#include "autoware/trajectory/interpolator/nearest_neighbor.hpp" -#include "autoware/trajectory/interpolator/stairstep.hpp" - -#include - -#include -#include - -int main() -{ - pybind11::scoped_interpreter guard{}; - auto plt = matplotlibcpp17::pyplot::import(); - - // create random values - std::vector bases = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0}; - std::vector values; - std::random_device seed_gen; - std::mt19937 engine(seed_gen()); - std::uniform_real_distribution<> dist(-1.0, 1.0); - for (size_t i = 0; i < bases.size(); ++i) { - values.push_back(dist(engine)); - } - // Scatter Data - plt.scatter(Args(bases, values)); - - using autoware::trajectory::interpolator::InterpolatorInterface; - // Linear Interpolator - { - using autoware::trajectory::interpolator::Linear; - auto interpolator = *Linear::Builder{}.set_bases(bases).set_values(values).build(); - std::vector x; - std::vector y; - for (double i = bases.front(); i < bases.back(); i += 0.01) { - x.push_back(i); - y.push_back(interpolator.compute(i)); - } - plt.plot(Args(x, y), Kwargs("label"_a = "Linear")); - } - - // AkimaSpline Interpolator - { - using autoware::trajectory::interpolator::AkimaSpline; - - auto interpolator = *AkimaSpline::Builder{}.set_bases(bases).set_values(values).build(); - std::vector x; - std::vector y; - for (double i = bases.front(); i < bases.back(); i += 0.01) { - x.push_back(i); - y.push_back(interpolator.compute(i)); - } - plt.plot(Args(x, y), Kwargs("label"_a = "AkimaSpline")); - } - - // CubicSpline Interpolator - { - using autoware::trajectory::interpolator::CubicSpline; - auto interpolator = *CubicSpline::Builder{}.set_bases(bases).set_values(values).build(); - std::vector x; - std::vector y; - for (double i = bases.front(); i < bases.back(); i += 0.01) { - x.push_back(i); - y.push_back(interpolator.compute(i)); - } - plt.plot(Args(x, y), Kwargs("label"_a = "CubicSpline")); - } - - // NearestNeighbor Interpolator - { - using autoware::trajectory::interpolator::NearestNeighbor; - auto interpolator = - *NearestNeighbor::Builder{}.set_bases(bases).set_values(values).build(); - std::vector x; - std::vector y; - for (double i = bases.front(); i < bases.back(); i += 0.01) { - x.push_back(i); - y.push_back(interpolator.compute(i)); - } - plt.plot(Args(x, y), Kwargs("label"_a = "NearestNeighbor")); - } - - // Stairstep Interpolator - { - using autoware::trajectory::interpolator::Stairstep; - auto interpolator = *Stairstep::Builder{}.set_bases(bases).set_values(values).build(); - std::vector x; - std::vector y; - for (double i = bases.front(); i < bases.back(); i += 0.01) { - x.push_back(i); - y.push_back(interpolator.compute(i)); - } - plt.plot(Args(x, y), Kwargs("label"_a = "Stairstep")); - } - - plt.legend(); - plt.show(); - return 0; -} diff --git a/common/autoware_trajectory/examples/example_shift.cpp b/common/autoware_trajectory/examples/example_shift.cpp deleted file mode 100644 index 9d04696f78f53..0000000000000 --- a/common/autoware_trajectory/examples/example_shift.cpp +++ /dev/null @@ -1,187 +0,0 @@ -// Copyright 2025 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/trajectory/point.hpp" -#include "autoware/trajectory/utils/shift.hpp" - -#include - -#include -#include - -geometry_msgs::msg::Point point(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(); - - geometry_msgs::msg::Point p; - (void)(p); - - std::vector points = { - point(0.49, 0.59), point(0.61, 1.22), point(0.86, 1.93), point(1.20, 2.56), point(1.51, 3.17), - point(1.85, 3.76), point(2.14, 4.26), point(2.60, 4.56), point(3.07, 4.55), point(3.61, 4.30), - point(3.95, 4.01), point(4.29, 3.68), point(4.90, 3.25), point(5.54, 3.10), point(6.24, 3.18), - point(6.88, 3.54), point(7.51, 4.25), point(7.85, 4.93), point(8.03, 5.73), point(8.16, 6.52), - point(8.31, 7.28), point(8.45, 7.93), point(8.68, 8.45), point(8.96, 8.96), point(9.32, 9.36)}; - - auto trajectory = - autoware::trajectory::Trajectory::Builder{}.build(points); - - if (!trajectory) { - return 1; - } - - std::cout << "length: " << trajectory->length() << std::endl; - - { - std::vector x; - std::vector y; - for (double s = 0.0; s < trajectory->length(); s += 0.01) { - auto p = trajectory->compute(s); - x.push_back(p.x); - y.push_back(p.y); - } - plt.plot(Args(x, y), Kwargs("label"_a = "original")); - - x.clear(); - y.clear(); - - autoware::trajectory::ShiftInterval shift_interval; - shift_interval.end = -1.0; - shift_interval.lateral_offset = 0.5; - - auto shifted_trajectory = autoware::trajectory::shift(*trajectory, shift_interval); - - for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) { - auto p = shifted_trajectory.compute(s); - x.push_back(p.x); - y.push_back(p.y); - } - - plt.axis(Args("equal")); - plt.plot(Args(x, y), Kwargs("label"_a = "shifted")); - plt.legend(); - plt.show(); - } - - { - std::vector x; - std::vector y; - for (double s = 0.0; s < trajectory->length(); s += 0.01) { - auto p = trajectory->compute(s); - x.push_back(p.x); - y.push_back(p.y); - } - plt.plot(Args(x, y), Kwargs("label"_a = "original")); - - x.clear(); - y.clear(); - - autoware::trajectory::ShiftInterval shift_interval; - shift_interval.start = trajectory->length() / 4.0; - shift_interval.end = trajectory->length() * 3.0 / 4.0; - shift_interval.lateral_offset = 0.5; - auto shifted_trajectory = autoware::trajectory::shift(*trajectory, shift_interval); - - for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) { - auto p = shifted_trajectory.compute(s); - x.push_back(p.x); - y.push_back(p.y); - } - - plt.axis(Args("equal")); - plt.plot(Args(x, y), Kwargs("label"_a = "shifted")); - plt.legend(); - plt.show(); - } - - { - std::vector x; - std::vector y; - for (double s = 0.0; s < trajectory->length(); s += 0.01) { - auto p = trajectory->compute(s); - x.push_back(p.x); - y.push_back(p.y); - } - plt.plot(Args(x, y), Kwargs("label"_a = "original")); - - x.clear(); - y.clear(); - - autoware::trajectory::ShiftInterval shift_interval; - shift_interval.start = trajectory->length() * 3.0 / 4.0; - shift_interval.end = trajectory->length() / 4.0; - shift_interval.lateral_offset = 0.5; - auto shifted_trajectory = autoware::trajectory::shift(*trajectory, shift_interval); - - for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) { - auto p = shifted_trajectory.compute(s); - x.push_back(p.x); - y.push_back(p.y); - } - - plt.axis(Args("equal")); - plt.plot(Args(x, y), Kwargs("label"_a = "shifted")); - plt.legend(); - plt.show(); - } - - { - std::vector x; - std::vector y; - for (double s = 0.0; s < trajectory->length(); s += 0.01) { - auto p = trajectory->compute(s); - x.push_back(p.x); - y.push_back(p.y); - } - plt.plot(Args(x, y), Kwargs("label"_a = "original")); - - x.clear(); - y.clear(); - - autoware::trajectory::ShiftInterval shift_interval1; - shift_interval1.start = trajectory->length() / 4.0; - shift_interval1.end = trajectory->length() * 2.0 / 4.0; - shift_interval1.lateral_offset = 0.5; - - autoware::trajectory::ShiftInterval shift_interval2; - shift_interval2.start = trajectory->length() * 2.0 / 4.0; - shift_interval2.end = trajectory->length() * 3.0 / 4.0; - shift_interval2.lateral_offset = -0.5; - - auto shifted_trajectory = - autoware::trajectory::shift(*trajectory, {shift_interval1, shift_interval2}); - - for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) { - auto p = shifted_trajectory.compute(s); - x.push_back(p.x); - y.push_back(p.y); - } - - plt.axis(Args("equal")); - plt.plot(Args(x, y), Kwargs("label"_a = "shifted")); - plt.legend(); - plt.show(); - } - - return 0; -} diff --git a/common/autoware_trajectory/examples/example_trajectory_path_point.cpp b/common/autoware_trajectory/examples/example_trajectory_path_point.cpp deleted file mode 100644 index 0d7317f09789e..0000000000000 --- a/common/autoware_trajectory/examples/example_trajectory_path_point.cpp +++ /dev/null @@ -1,118 +0,0 @@ -// 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/trajectory/path_point.hpp" -#include "autoware/trajectory/utils/crossed.hpp" -#include "lanelet2_core/primitives/LineString.h" - -#include -#include - -#include - -#include -#include - -#include -#include - -autoware_planning_msgs::msg::PathPoint path_point(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::trajectory::Trajectory; - - pybind11::scoped_interpreter guard{}; - - auto plt = matplotlibcpp17::pyplot::import(); - - std::vector points = { - path_point(0.49, 0.59), path_point(0.61, 1.22), path_point(0.86, 1.93), path_point(1.20, 2.56), - path_point(1.51, 3.17), path_point(1.85, 3.76), path_point(2.14, 4.26), path_point(2.60, 4.56), - path_point(3.07, 4.55), path_point(3.61, 4.30), path_point(3.95, 4.01), path_point(4.29, 3.68), - path_point(4.90, 3.25), path_point(5.54, 3.10), path_point(6.24, 3.18), path_point(6.88, 3.54), - path_point(7.51, 4.25), path_point(7.85, 4.93), path_point(8.03, 5.73), path_point(8.16, 6.52), - path_point(8.31, 7.28), path_point(8.45, 7.93), path_point(8.68, 8.45), path_point(8.96, 8.96), - path_point(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 = Trajectory::Builder{}.build(points); - - if (!trajectory) { - std::cerr << "Failed to build trajectory" << std::endl; - return 1; - } - - trajectory->align_orientation_with_trajectory_direction(); - - lanelet::LineString2d line_string; - line_string.push_back(lanelet::Point3d(lanelet::InvalId, 7.5, 8.6, 0.0)); - line_string.push_back(lanelet::Point3d(lanelet::InvalId, 10.2, 7.7, 0.0)); - - auto s = autoware::trajectory::crossed(*trajectory, line_string); - auto crossed = trajectory->compute(s.at(0)); - - plt.plot( - Args( - std::vector{line_string[0].x(), line_string[1].x()}, - std::vector{line_string[0].y(), line_string[1].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().range(s.at(0), trajectory->length()).set(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_trajectory/examples/example_trajectory_point.cpp b/common/autoware_trajectory/examples/example_trajectory_point.cpp deleted file mode 100644 index 3aead351085ee..0000000000000 --- a/common/autoware_trajectory/examples/example_trajectory_point.cpp +++ /dev/null @@ -1,140 +0,0 @@ -// 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/trajectory/interpolator/cubic_spline.hpp" -#include "autoware/trajectory/point.hpp" -#include "autoware/trajectory/utils/closest.hpp" -#include "autoware/trajectory/utils/crossed.hpp" -#include "lanelet2_core/primitives/LineString.h" - -#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::trajectory::Trajectory; - using autoware::trajectory::interpolator::CubicSpline; - - auto trajectory = Trajectory::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; - - double s = autoware::trajectory::closest(*trajectory, 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")); - } - { - lanelet::LineString2d line_string; - line_string.push_back(lanelet::Point3d(lanelet::InvalId, 6.97, 6.36, 0.0)); - line_string.push_back(lanelet::Point3d(lanelet::InvalId, 9.23, 5.92, 0.0)); - - auto s = autoware::trajectory::crossed(*trajectory, line_string); - if (s.empty()) { - std::cerr << "Failed to find a crossing point" << std::endl; - return 1; - } - auto crossed = trajectory->compute(s.at(0)); - - plt.plot( - Args( - std::vector{line_string[0].x(), line_string[1].x()}, - std::vector{line_string[0].y(), line_string[1].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_trajectory/examples/example_trajectory_pose.cpp b/common/autoware_trajectory/examples/example_trajectory_pose.cpp deleted file mode 100644 index baa26abedecc1..0000000000000 --- a/common/autoware_trajectory/examples/example_trajectory_pose.cpp +++ /dev/null @@ -1,92 +0,0 @@ -// 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/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::trajectory::Trajectory; - - auto trajectory = Trajectory::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_trajectory/include/autoware/trajectory/detail/helpers.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/helpers.hpp deleted file mode 100644 index 55981326c4db4..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/detail/helpers.hpp +++ /dev/null @@ -1,72 +0,0 @@ -// 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__TRAJECTORY__DETAIL__HELPERS_HPP_ -#define AUTOWARE__TRAJECTORY__DETAIL__HELPERS_HPP_ - -#include -#include -#include - -namespace autoware::trajectory::detail -{ -inline namespace helpers -{ -/** - * @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()}; -} - -/** - * @brief Ensures the vector has at least a specified number of points by linearly interpolating - * values. - * - * @param x Input vector of double values. - * @param min_points Minimum number of points required. - * @return A vector with at least `min_points` elements. - * - * @note If `x.size() >= min_points`, the input vector is returned as-is. - * - * @code - * std::vector input = {1.0, 4.0, 6.0}; - * auto result = fill_bases(input, 6); - * // result: {1.0, 2.0, 3.0, 4.0, 5.0, 6.0} - * @endcode - */ -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 helpers -} // namespace autoware::trajectory::detail - -#endif // AUTOWARE__TRAJECTORY__DETAIL__HELPERS_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp deleted file mode 100644 index 06f1185620692..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp +++ /dev/null @@ -1,217 +0,0 @@ -// 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__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ -#define AUTOWARE__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ - -#include "autoware/trajectory/detail/logging.hpp" -#include "autoware/trajectory/interpolator/interpolator.hpp" - -#include -#include - -#include -#include -#include -#include - -namespace autoware::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) - { - if (this != &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: - void set(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); - - bool success = parent_.interpolator_->build(bases, values); - if (!success) { - throw std::runtime_error( - "Failed to build interpolator."); // This Exception should not be thrown. - } - } - }; - - /** - * @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 range(double start, double end) - { - if (start < this->start() || end > this->end()) { - RCLCPP_WARN( - get_logger(), "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); - bool success = interpolator_->build(bases_, values_); - if (!success) { - throw std::runtime_error( - "Failed to build interpolator."); // This Exception should not be thrown. - } - return *this; - } - - /** - * @brief Compute the interpolated value at a given position. - * @param x The position to compute the value at. - * @return The interpolated value. - */ - [[nodiscard]] 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. - */ - [[nodiscard]] std::pair, std::vector> get_data() const - { - return {bases_, values_}; - } -}; - -} // namespace autoware::trajectory::detail - -#endif // AUTOWARE__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/detail/logging.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/logging.hpp deleted file mode 100644 index 2ad8d66a4c46d..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/detail/logging.hpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright 2025 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__TRAJECTORY__DETAIL__LOGGING_HPP_ -#define AUTOWARE__TRAJECTORY__DETAIL__LOGGING_HPP_ - -#include -#include - -namespace autoware::trajectory -{ - -rclcpp::Logger & get_logger(); - -rclcpp::Clock & get_clock(); - -} // namespace autoware::trajectory - -#endif // AUTOWARE__TRAJECTORY__DETAIL__LOGGING_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp deleted file mode 100644 index f4150b7e37294..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/detail/types.hpp +++ /dev/null @@ -1,77 +0,0 @@ -// 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__TRAJECTORY__DETAIL__TYPES_HPP_ -#define AUTOWARE__TRAJECTORY__DETAIL__TYPES_HPP_ - -#include "lanelet2_core/primitives/Point.h" - -#include - -#include -#include -#include -#include - -namespace autoware::trajectory::detail -{ - -struct MutablePoint2d -{ - MutablePoint2d(double & x, double & y) : x(x), y(y) {} - double & x; - double & y; -}; -struct MutablePoint3d : MutablePoint2d -{ - MutablePoint3d(double & x, double & y, double & z) : MutablePoint2d{x, y}, z(z) {} - double & z; -}; - -struct ImmutablePoint2d -{ - ImmutablePoint2d(const double & x, const double & y) : x(x), y(y) {} - const double x; - const double y; -}; - -struct ImmutablePoint3d : ImmutablePoint2d -{ - ImmutablePoint3d(const double & x, const double & y, const double & z) - : ImmutablePoint2d{x, y}, z(z) - { - } - const double z; -}; - -/** - * @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. - */ -MutablePoint3d to_point(geometry_msgs::msg::Point & p); -MutablePoint3d to_point(geometry_msgs::msg::Pose & p); -MutablePoint3d to_point(autoware_planning_msgs::msg::PathPoint & p); -MutablePoint3d to_point(autoware_internal_planning_msgs::msg::PathPointWithLaneId & p); -MutablePoint2d to_point(lanelet::BasicPoint2d & p); - -ImmutablePoint3d to_point(const geometry_msgs::msg::Point & p); -ImmutablePoint3d to_point(const geometry_msgs::msg::Pose & p); -ImmutablePoint3d to_point(const autoware_planning_msgs::msg::PathPoint & p); -ImmutablePoint3d to_point(const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p); -ImmutablePoint2d to_point(const lanelet::BasicPoint2d & p); - -} // namespace autoware::trajectory::detail - -#endif // AUTOWARE__TRAJECTORY__DETAIL__TYPES_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/forward.hpp b/common/autoware_trajectory/include/autoware/trajectory/forward.hpp deleted file mode 100644 index 1bd9d1e4705e2..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/forward.hpp +++ /dev/null @@ -1,26 +0,0 @@ -// 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__TRAJECTORY__FORWARD_HPP_ -#define AUTOWARE__TRAJECTORY__FORWARD_HPP_ - -namespace autoware::trajectory -{ - -template -class Trajectory; - -} // namespace autoware::trajectory - -#endif // AUTOWARE__TRAJECTORY__FORWARD_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/akima_spline.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/akima_spline.hpp deleted file mode 100644 index e3484b0205200..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/akima_spline.hpp +++ /dev/null @@ -1,94 +0,0 @@ -// 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__TRAJECTORY__INTERPOLATOR__AKIMA_SPLINE_HPP_ -#define AUTOWARE__TRAJECTORY__INTERPOLATOR__AKIMA_SPLINE_HPP_ - -#include "autoware/trajectory/interpolator/detail/interpolator_mixin.hpp" - -#include - -#include - -namespace autoware::trajectory::interpolator -{ - -/** - * @brief Class for Akima spline interpolation. - * - * This class provides methods to perform Akima spline interpolation on a set of data points. - */ -class AkimaSpline : public detail::InterpolatorMixin -{ -private: - Eigen::VectorXd a_, b_, c_, d_; ///< Coefficients for the Akima spline. - - /** - * @brief Compute the spline parameters. - * - * This method computes the coefficients for the Akima spline. - * - * @param bases The bases values. - * @param values The values to interpolate. - */ - void compute_parameters( - const Eigen::Ref & bases, - const Eigen::Ref & values); - - /** - * @brief Build the interpolator with the given values. - * - * @param bases The bases values. - * @param values The values to interpolate. - */ - void build_impl(const std::vector & bases, const std::vector & values) override; - - /** - * @brief Compute the interpolated value at the given point. - * - * @param s The point at which to compute the interpolated value. - * @return The interpolated value. - */ - [[nodiscard]] double compute_impl(const double & s) const override; - - /** - * @brief Compute the first derivative at the given point. - * - * @param s The point at which to compute the first derivative. - * @return The first derivative. - */ - [[nodiscard]] double compute_first_derivative_impl(const double & s) const override; - - /** - * @brief Compute the second derivative at the given point. - * - * @param s The point at which to compute the second derivative. - * @return The second derivative. - */ - [[nodiscard]] double compute_second_derivative_impl(const double & s) const override; - -public: - AkimaSpline() = default; - - /** - * @brief Get the minimum number of required points for the interpolator. - * - * @return The minimum number of required points. - */ - [[nodiscard]] size_t minimum_required_points() const override { return 5; } -}; - -} // namespace autoware::trajectory::interpolator - -#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__AKIMA_SPLINE_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/cubic_spline.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/cubic_spline.hpp deleted file mode 100644 index 51694b15f8932..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/cubic_spline.hpp +++ /dev/null @@ -1,96 +0,0 @@ -// 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__TRAJECTORY__INTERPOLATOR__CUBIC_SPLINE_HPP_ -#define AUTOWARE__TRAJECTORY__INTERPOLATOR__CUBIC_SPLINE_HPP_ - -#include "autoware/trajectory/interpolator/detail/interpolator_mixin.hpp" - -#include - -#include - -namespace autoware::trajectory::interpolator -{ - -/** - * @brief Class for cubic spline interpolation. - * - * This class provides methods to perform cubic spline interpolation on a set of data points. - */ -class CubicSpline : public detail::InterpolatorMixin -{ -private: - Eigen::VectorXd a_, b_, c_, d_; ///< Coefficients for the cubic spline. - Eigen::VectorXd h_; ///< Interval sizes between bases points. - - /** - * @brief Compute the spline parameters. - * - * This method computes the coefficients for the cubic spline. - * - * @param bases The bases values. - * @param values The values to interpolate. - */ - void compute_parameters( - const Eigen::Ref & bases, - const Eigen::Ref & values); - - /** - * @brief Build the interpolator with the given values. - * - * @param bases The bases values. - * @param values The values to interpolate. - * @return True if the interpolator was built successfully, false otherwise. - */ - void build_impl(const std::vector & bases, const std::vector & values) override; - - /** - * @brief Compute the interpolated value at the given point. - * - * @param s The point at which to compute the interpolated value. - * @return The interpolated value. - */ - [[nodiscard]] double compute_impl(const double & s) const override; - - /** - * @brief Compute the first derivative at the given point. - * - * @param s The point at which to compute the first derivative. - * @return The first derivative. - */ - [[nodiscard]] double compute_first_derivative_impl(const double & s) const override; - - /** - * @brief Compute the second derivative at the given point. - * - * @param s The point at which to compute the second derivative. - * @return The second derivative. - */ - [[nodiscard]] double compute_second_derivative_impl(const double & s) const override; - -public: - CubicSpline() = default; - - /** - * @brief Get the minimum number of required points for the interpolator. - * - * @return The minimum number of required points. - */ - [[nodiscard]] size_t minimum_required_points() const override { return 4; } -}; - -} // namespace autoware::trajectory::interpolator - -#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__CUBIC_SPLINE_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_common_interface.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_common_interface.hpp deleted file mode 100644 index df18c65e0520b..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_common_interface.hpp +++ /dev/null @@ -1,176 +0,0 @@ -// 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__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_ -#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_ - -#include -#include - -#include - -namespace autoware::trajectory::interpolator::detail -{ -/** - * @brief Base class for interpolation implementations. - * - * This class provides the basic interface and common functionality for different types - * of interpolation. It is intended to be subclassed by specific interpolation algorithms. - * - * @tparam T The type of the values being interpolated. - */ -template -class InterpolatorCommonInterface -{ -protected: - std::vector bases_; ///< bases values for the interpolation. - bool is_built_{false}; ///< flag indicating whether the interpolator has been built. - - /** - * @brief Get the start of the interpolation range. - */ - [[nodiscard]] double start() const { return bases_.front(); } - - /** - * @brief Get the end of the interpolation range. - */ - [[nodiscard]] double end() const { return bases_.back(); } - - /** - * @brief Compute the interpolated value at the given point. - * - * This method should be overridden by subclasses to provide the specific interpolation logic. - * - * @param s The point at which to compute the interpolated value. - * @return The interpolated value. - */ - [[nodiscard]] virtual T compute_impl(const double & s) const = 0; - - /** - * @brief Build the interpolator with the given values. - * - * This method should be overridden by subclasses to provide the specific build logic. - * - * @param bases The bases values. - * @param values The values to interpolate. - */ - virtual void build_impl(const std::vector & bases, const std::vector & values) = 0; - - /** - * @brief Validate the input to the compute method. - * - * Checks that the interpolator has been built and that the input value is within range. - * - * @param s The input value. - * @return The input value, clamped to the range of the interpolator. - */ - [[nodiscard]] double validate_compute_input(const double & s) const - { - if (s < start() || s > end()) { - RCLCPP_WARN( - rclcpp::get_logger("Interpolator"), - "Input value %f is outside the range of the interpolator [%f, %f].", s, start(), end()); - } - return std::clamp(s, start(), end()); - } - - /** - * @brief Get the index of the interval containing the input value. - * - * This method determines the index of the interval in the bases array that contains the given - * value. It assumes that the bases array is sorted in ascending order. - * - * If `end_inclusive` is true and the input value matches the end of the bases array, - * the method returns the index of the second-to-last interval. - * - * @param s The input value for which to find the interval index. - * @param end_inclusive Whether to include the end value in the last interval. Defaults to true. - * @return The index of the interval containing the input value. - * - * @throw std::out_of_range if the input value is outside the range of the bases array. - */ - [[nodiscard]] int32_t get_index(const double & s, bool end_inclusive = true) const - { - if (end_inclusive && s == end()) { - return static_cast(bases_.size()) - 2; - } - auto comp = [](const double & a, const double & b) { return a <= b; }; - return std::distance(bases_.begin(), std::lower_bound(bases_.begin(), bases_.end(), s, comp)) - - 1; - } - -public: - InterpolatorCommonInterface() = default; - virtual ~InterpolatorCommonInterface() = default; - InterpolatorCommonInterface(const InterpolatorCommonInterface & other) = default; - InterpolatorCommonInterface & operator=(const InterpolatorCommonInterface & other) = default; - InterpolatorCommonInterface(InterpolatorCommonInterface && other) noexcept = default; - InterpolatorCommonInterface & operator=(InterpolatorCommonInterface && other) noexcept = default; - - /** - * @brief Build the interpolator with the given bases and values. - * - * @param bases The bases values. - * @param values The values to interpolate. - * @return True if the interpolator was built successfully, false otherwise. - */ - [[nodiscard]] bool build(const std::vector & bases, const std::vector & values) - { - if (bases.size() != values.size()) { - return false; - } - if (bases.size() < minimum_required_points()) { - return false; - } - build_impl(bases, values); - is_built_ = true; - return true; - } - - /** - * @brief Check if the interpolator has been built. - * - * @return True if the interpolator has been built, false otherwise. - */ - [[nodiscard]] bool is_built() const { return is_built_; } - - /** - * @brief Get the minimum number of required points for the interpolator. - * - * This method should be overridden by subclasses to return the specific requirement. - * - * @return The minimum number of required points. - */ - [[nodiscard]] virtual size_t minimum_required_points() const = 0; - - /** - * @brief Compute the interpolated value at the given point. - * - * @param s The point at which to compute the interpolated value. - * @return The interpolated value. - * @throw std::runtime_error if the interpolator has not been built. - */ - [[nodiscard]] T compute(const double & s) const - { - if (!is_built_) { - throw std::runtime_error( - "Interpolator has not been built."); // This Exception should not be thrown. - } - const double clamped_s = validate_compute_input(s); - return compute_impl(clamped_s); - } -}; -} // namespace autoware::trajectory::interpolator::detail - -#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_mixin.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_mixin.hpp deleted file mode 100644 index 98e46e5467a89..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_mixin.hpp +++ /dev/null @@ -1,86 +0,0 @@ -// 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__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_ -#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_ - -#include "autoware/trajectory/interpolator/interpolator.hpp" - -#include - -#include -#include -#include -#include - -namespace autoware::trajectory::interpolator::detail -{ - -/** - * @brief Base class for interpolator implementations. - * - * This class implements the core functionality for interpolator implementations. - * - * @tparam InterpolatorType The type of the interpolator implementation. - * @tparam T The type of the values being interpolated. - */ -template -struct InterpolatorMixin : public InterpolatorInterface -{ - [[nodiscard]] std::shared_ptr> clone() const override - { - return std::make_shared(static_cast(*this)); - } - - class Builder - { - private: - std::vector bases_; - std::vector values_; - - public: - [[nodiscard]] Builder & set_bases(const Eigen::Ref & bases) - { - bases_ = std::vector(bases.begin(), bases.end()); - return *this; - } - - [[nodiscard]] Builder & set_bases(const std::vector & bases) - { - bases_ = bases; - return *this; - } - - [[nodiscard]] Builder & set_values(const std::vector & values) - { - values_ = values; - return *this; - } - - template - [[nodiscard]] std::optional build(Args &&... args) - { - auto interpolator = InterpolatorType(std::forward(args)...); - const bool success = interpolator.build(bases_, values_); - if (!success) { - return std::nullopt; - } - return interpolator; - } - }; -}; - -} // namespace autoware::trajectory::interpolator::detail - -#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/nearest_neighbor_common_impl.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/nearest_neighbor_common_impl.hpp deleted file mode 100644 index f02ed5fa8a4c4..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/nearest_neighbor_common_impl.hpp +++ /dev/null @@ -1,82 +0,0 @@ -// 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__TRAJECTORY__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_ -#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_ - -#include "autoware/trajectory/interpolator/detail/interpolator_mixin.hpp" - -#include - -namespace autoware::trajectory::interpolator -{ - -template -class NearestNeighbor; - -namespace detail -{ -/** - * @brief Common Implementation of nearest neighbor. - * - * This class implements the core functionality for nearest neighbor interpolation. - * - * @tparam T The type of the values being interpolated. - */ -template -class NearestNeighborCommonImpl : public detail::InterpolatorMixin, T> -{ -protected: - std::vector values_; ///< Interpolation values. - - /** - * @brief Compute the interpolated value at the given point. - * - * @param s The point at which to compute the interpolated value. - * @return The interpolated value. - */ - [[nodiscard]] T compute_impl(const double & s) const override - { - const int32_t idx = this->get_index(s); - return (std::abs(s - this->bases_[idx]) <= std::abs(s - this->bases_[idx + 1])) - ? this->values_.at(idx) - : this->values_.at(idx + 1); - } - - /** - * @brief Build the interpolator with the given values. - * - * @param bases The bases values. - * @param values The values to interpolate. - * @return True if the interpolator was built successfully, false otherwise. - */ - void build_impl(const std::vector & bases, const std::vector & values) override - { - this->bases_ = bases; - this->values_ = values; - } - -public: - /** - * @brief Get the minimum number of required points for the interpolator. - * - * @return The minimum number of required points. - */ - [[nodiscard]] size_t minimum_required_points() const override { return 1; } -}; - -} // namespace detail -} // namespace autoware::trajectory::interpolator - -#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/stairstep_common_impl.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/stairstep_common_impl.hpp deleted file mode 100644 index 5b6a6b69429ef..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/stairstep_common_impl.hpp +++ /dev/null @@ -1,81 +0,0 @@ -// 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__TRAJECTORY__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_ -#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_ - -#include "autoware/trajectory/interpolator/detail/interpolator_mixin.hpp" - -#include - -namespace autoware::trajectory::interpolator -{ - -template -class Stairstep; - -namespace detail -{ - -/** - * @brief Base class for stairstep interpolation. - * - * This class implements the core functionality for stairstep interpolation. - * - * @tparam T The type of the values being interpolated. - */ -template -class StairstepCommonImpl : public detail::InterpolatorMixin, T> -{ -protected: - std::vector values_; ///< Interpolation values. - - /** - * @brief Compute the interpolated value at the given point. - * - * @param s The point at which to compute the interpolated value. - * @return The interpolated value. - */ - [[nodiscard]] T compute_impl(const double & s) const override - { - const int32_t idx = this->get_index(s, false); - return this->values_.at(idx); - } - /** - * @brief Build the interpolator with the given values. - * - * @param bases The bases values. - * @param values The values to interpolate. - */ - void build_impl(const std::vector & bases, const std::vector & values) override - { - this->bases_ = bases; - this->values_ = values; - } - -public: - /** - * @brief Default constructor. - */ - StairstepCommonImpl() = default; - - /** - * @brief Get the minimum number of required points for the interpolator. - */ - [[nodiscard]] size_t minimum_required_points() const override { return 2; } -}; -} // namespace detail -} // namespace autoware::trajectory::interpolator - -#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp deleted file mode 100644 index 1f207822e23a0..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp +++ /dev/null @@ -1,97 +0,0 @@ -// 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__TRAJECTORY__INTERPOLATOR__INTERPOLATOR_HPP_ -#define AUTOWARE__TRAJECTORY__INTERPOLATOR__INTERPOLATOR_HPP_ - -#include "autoware/trajectory/interpolator/detail/interpolator_common_interface.hpp" - -#include - -namespace autoware::trajectory::interpolator -{ -/** - * @brief Template class for interpolation. - * - * This class serves as the base class for specific interpolation types. - * - * @tparam T The type of the values being interpolated. (e.g. double, int, etc.) - */ -template -class InterpolatorInterface : public detail::InterpolatorCommonInterface -{ -public: - [[nodiscard]] virtual std::shared_ptr> clone() const = 0; -}; - -/** - * @brief Specialization of Interpolator for double values. - * - * This class adds methods for computing first and second derivatives. - */ -template <> -class InterpolatorInterface : public detail::InterpolatorCommonInterface -{ -protected: - /** - * @brief Compute the first derivative at the given point. - * - * This method should be overridden by subclasses to provide the specific logic. - * - * @param s The point at which to compute the first derivative. - * @return The first derivative. - */ - [[nodiscard]] virtual double compute_first_derivative_impl(const double & s) const = 0; - - /** - * @brief Compute the second derivative at the given point. - * - * This method should be overridden by subclasses to provide the specific logic. - * - * @param s The point at which to compute the second derivative. - * @return The second derivative. - */ - [[nodiscard]] virtual double compute_second_derivative_impl(const double & s) const = 0; - -public: - /** - * @brief Compute the first derivative at the given point. - * - * @param s The point at which to compute the first derivative. - * @return The first derivative. - */ - [[nodiscard]] double compute_first_derivative(const double & s) const - { - const double clamped_s = this->validate_compute_input(s); - return compute_first_derivative_impl(clamped_s); - } - - /** - * @brief Compute the second derivative at the given point. - * - * @param s The point at which to compute the second derivative. - * @return The second derivative. - */ - [[nodiscard]] double compute_second_derivative(const double & s) const - { - const double clamped_s = this->validate_compute_input(s); - return compute_second_derivative_impl(clamped_s); - } - - [[nodiscard]] virtual std::shared_ptr> clone() const = 0; -}; - -} // namespace autoware::trajectory::interpolator - -#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__INTERPOLATOR_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/linear.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/linear.hpp deleted file mode 100644 index bef2cde70fff7..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/linear.hpp +++ /dev/null @@ -1,86 +0,0 @@ -// 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__TRAJECTORY__INTERPOLATOR__LINEAR_HPP_ -#define AUTOWARE__TRAJECTORY__INTERPOLATOR__LINEAR_HPP_ - -#include "autoware/trajectory/interpolator/detail/interpolator_mixin.hpp" - -#include - -#include - -namespace autoware::trajectory::interpolator -{ - -/** - * @brief Class for linear interpolation. - * - * This class provides methods to perform linear interpolation on a set of data points. - */ -class Linear : public detail::InterpolatorMixin -{ -private: - Eigen::VectorXd values_; ///< Interpolation values. - - /** - * @brief Build the interpolator with the given values. - * - * @param bases The bases values. - * @param values The values to interpolate. - * @return True if the interpolator was built successfully, false otherwise. - */ - void build_impl(const std::vector & bases, const std::vector & values) override; - - /** - * @brief Compute the interpolated value at the given point. - * - * @param s The point at which to compute the interpolated value. - * @return The interpolated value. - */ - [[nodiscard]] double compute_impl(const double & s) const override; - - /** - * @brief Compute the first derivative at the given point. - * - * @param s The point at which to compute the first derivative. - * @return The first derivative. - */ - [[nodiscard]] double compute_first_derivative_impl(const double & s) const override; - - /** - * @brief Compute the second derivative at the given point. - * - * @param s The point at which to compute the second derivative. - * @return The second derivative. - */ - [[nodiscard]] double compute_second_derivative_impl(const double &) const override; - -public: - /** - * @brief Default constructor. - */ - Linear() = default; - - /** - * @brief Get the minimum number of required points for the interpolator. - * - * @return The minimum number of required points. - */ - [[nodiscard]] size_t minimum_required_points() const override; -}; - -} // namespace autoware::trajectory::interpolator - -#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__LINEAR_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/nearest_neighbor.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/nearest_neighbor.hpp deleted file mode 100644 index 959c8cfb3a5fb..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/nearest_neighbor.hpp +++ /dev/null @@ -1,78 +0,0 @@ -// 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__TRAJECTORY__INTERPOLATOR__NEAREST_NEIGHBOR_HPP_ -#define AUTOWARE__TRAJECTORY__INTERPOLATOR__NEAREST_NEIGHBOR_HPP_ - -#include "autoware/trajectory/interpolator/detail/nearest_neighbor_common_impl.hpp" - -namespace autoware::trajectory::interpolator -{ - -/** - * @brief Template class for nearest neighbor interpolation. - * - * This class provides methods to perform nearest neighbor interpolation on a set of data points. - * - * @tparam T The type of the values being interpolated. - */ -template -class NearestNeighbor; - -/** - * @brief Template class for nearest neighbor interpolation. - * - * This class provides the interface for nearest neighbor interpolation. - * - * @tparam T The type of the values being interpolated. - */ -template -class NearestNeighbor : public detail::NearestNeighborCommonImpl -{ -public: - NearestNeighbor() = default; -}; - -/** - * @brief Specialization of NearestNeighbor for double values. - * - * This class provides methods to perform nearest neighbor interpolation on double values. - */ -template <> -class NearestNeighbor : public detail::NearestNeighborCommonImpl -{ -private: - /** - * @brief Compute the first derivative at the given point. - * - * @param s The point at which to compute the first derivative. - * @return The first derivative. - */ - [[nodiscard]] double compute_first_derivative_impl(const double &) const override { return 0.0; } - - /** - * @brief Compute the second derivative at the given point. - * - * @param s The point at which to compute the second derivative. - * @return The second derivative. - */ - [[nodiscard]] double compute_second_derivative_impl(const double &) const override { return 0.0; } - -public: - NearestNeighbor() = default; -}; - -} // namespace autoware::trajectory::interpolator - -#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__NEAREST_NEIGHBOR_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/spherical_linear.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/spherical_linear.hpp deleted file mode 100644 index c443daf114d0d..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/spherical_linear.hpp +++ /dev/null @@ -1,73 +0,0 @@ -// 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__TRAJECTORY__INTERPOLATOR__SPHERICAL_LINEAR_HPP_ -#define AUTOWARE__TRAJECTORY__INTERPOLATOR__SPHERICAL_LINEAR_HPP_ - -#include "autoware/trajectory/interpolator/detail/interpolator_mixin.hpp" - -#include - -#include - -namespace autoware::trajectory::interpolator -{ - -/** - * @brief Class for SphericalLinear interpolation. - * - * This class provides methods to perform SphericalLinear interpolation on a set of data points. - */ -class SphericalLinear -: public detail::InterpolatorMixin -{ -private: - std::vector quaternions_; - - /** - * @brief Build the interpolator with the given values. - * - * @param bases The bases values. - * @param values The values to interpolate. - * @return True if the interpolator was built successfully, false otherwise. - */ - void build_impl( - const std::vector & bases, - const std::vector & quaternions) override; - - /** - * @brief Compute the interpolated value at the given point. - * - * @param s The point at which to compute the interpolated value. - * @return The interpolated value. - */ - [[nodiscard]] geometry_msgs::msg::Quaternion compute_impl(const double & s) const override; - -public: - /** - * @brief Default constructor. - */ - SphericalLinear() = default; - - /** - * @brief Get the minimum number of required points for the interpolator. - * - * @return The minimum number of required points. - */ - [[nodiscard]] size_t minimum_required_points() const override; -}; - -} // namespace autoware::trajectory::interpolator - -#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__SPHERICAL_LINEAR_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/interpolator/stairstep.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/stairstep.hpp deleted file mode 100644 index ad7ac8c7e53af..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/interpolator/stairstep.hpp +++ /dev/null @@ -1,78 +0,0 @@ -// 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__TRAJECTORY__INTERPOLATOR__STAIRSTEP_HPP_ -#define AUTOWARE__TRAJECTORY__INTERPOLATOR__STAIRSTEP_HPP_ - -#include "autoware/trajectory/interpolator/detail/stairstep_common_impl.hpp" - -namespace autoware::trajectory::interpolator -{ - -/** - * @brief Template class for stairstep interpolation. - * - * This class provides methods to perform stairstep interpolation on a set of data points. - * - * @tparam T The type of the values being interpolated. - */ -template -class Stairstep; - -/** - * @brief Template class for stairstep interpolation. - * - * This class provides the interface for stairstep interpolation. - * - * @tparam T The type of the values being interpolated. - */ -template -class Stairstep : public detail::StairstepCommonImpl -{ -public: - Stairstep() = default; -}; - -/** - * @brief Specialization of Stairstep for double values. - * - * This class provides methods to perform stairstep interpolation on double values. - */ -template <> -class Stairstep : public detail::StairstepCommonImpl -{ -private: - /** - * @brief Compute the first derivative at the given point. - * - * @param s The point at which to compute the first derivative. - * @return The first derivative. - */ - [[nodiscard]] double compute_first_derivative_impl(const double &) const override { return 0.0; } - - /** - * @brief Compute the second derivative at the given point. - * - * @param s The point at which to compute the second derivative. - * @return The second derivative. - */ - [[nodiscard]] double compute_second_derivative_impl(const double &) const override { return 0.0; } - -public: - Stairstep() = default; -}; - -} // namespace autoware::trajectory::interpolator - -#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__STAIRSTEP_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp b/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp deleted file mode 100644 index d7295570b53b9..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp +++ /dev/null @@ -1,170 +0,0 @@ -// 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__TRAJECTORY__PATH_POINT_HPP_ -#define AUTOWARE__TRAJECTORY__PATH_POINT_HPP_ - -#include "autoware/trajectory/detail/interpolated_array.hpp" -#include "autoware/trajectory/pose.hpp" - -#include - -#include -#include -#include - -namespace autoware::trajectory -{ -template <> -class Trajectory -: public Trajectory -{ - using BaseClass = Trajectory; - using PointType = autoware_planning_msgs::msg::PathPoint; - - std::shared_ptr> - longitudinal_velocity_mps_; //!< Longitudinal velocity in m/s - std::shared_ptr> - lateral_velocity_mps_; //!< Lateral velocity in m/s - std::shared_ptr> - heading_rate_rps_; //!< Heading rate in rad/s}; - -public: - Trajectory(); - ~Trajectory() override = default; - Trajectory(const Trajectory & rhs); - Trajectory(Trajectory && rhs) = default; - Trajectory & operator=(const Trajectory & rhs); - Trajectory & operator=(Trajectory && rhs) = default; - - [[nodiscard]] std::vector get_internal_bases() const override; - - detail::InterpolatedArray & longitudinal_velocity_mps() - { - return *longitudinal_velocity_mps_; - } - - detail::InterpolatedArray & lateral_velocity_mps() { return *lateral_velocity_mps_; } - - detail::InterpolatedArray & heading_rate_rps() { return *heading_rate_rps_; } - - [[nodiscard]] const detail::InterpolatedArray & longitudinal_velocity_mps() const - { - return *longitudinal_velocity_mps_; - } - - [[nodiscard]] const detail::InterpolatedArray & lateral_velocity_mps() const - { - return *lateral_velocity_mps_; - } - - [[nodiscard]] const detail::InterpolatedArray & heading_rate_rps() const - { - return *heading_rate_rps_; - } - - /** - * @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 = 4) const; - - class Builder - { - private: - std::unique_ptr trajectory_; - - public: - Builder() : trajectory_(std::make_unique()) {} - - 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_ = std::make_shared>( - std::make_shared(std::forward(args)...)); - return *this; - } - - template - Builder & set_lateral_velocity_interpolator(Args &&... args) - { - trajectory_->lateral_velocity_mps_ = std::make_shared>( - std::make_shared(std::forward(args)...)); - return *this; - } - - template - Builder & set_heading_rate_interpolator(Args &&... args) - { - trajectory_->heading_rate_rps_ = std::make_shared>( - 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::trajectory - -#endif // AUTOWARE__TRAJECTORY__PATH_POINT_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp b/common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp deleted file mode 100644 index d70d8443a511d..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp +++ /dev/null @@ -1,155 +0,0 @@ -// 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__TRAJECTORY__PATH_POINT_WITH_LANE_ID_HPP_ -#define AUTOWARE__TRAJECTORY__PATH_POINT_WITH_LANE_ID_HPP_ - -#include "autoware/trajectory/path_point.hpp" - -#include - -#include -#include -#include - -namespace autoware::trajectory -{ -template <> -class Trajectory -: public Trajectory -{ - using BaseClass = Trajectory; - using PointType = autoware_internal_planning_msgs::msg::PathPointWithLaneId; - using LaneIdType = std::vector; - - std::shared_ptr> lane_ids_; //!< Lane ID - -public: - Trajectory(); - ~Trajectory() override = default; - Trajectory(const Trajectory & rhs) = default; - Trajectory(Trajectory && rhs) = default; - Trajectory & operator=(const Trajectory & rhs); - Trajectory & operator=(Trajectory && rhs) = default; - - detail::InterpolatedArray & lane_ids() { return *lane_ids_; } - - [[nodiscard]] const detail::InterpolatedArray & lane_ids() const - { - return *lane_ids_; - } - - /** - * @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); - - [[nodiscard]] std::vector get_internal_bases() const override; - - /** - * @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 = 4) const; - - class Builder - { - private: - std::unique_ptr trajectory_; - - public: - Builder() : trajectory_(std::make_unique()) {} - - 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_ = std::make_shared>( - std::make_shared(std::forward(args)...)); - return *this; - } - - template - Builder & set_lateral_velocity_interpolator(Args &&... args) - { - trajectory_->lateral_velocity_mps_ = std::make_shared>( - std::make_shared(std::forward(args)...)); - return *this; - } - - template - Builder & set_heading_rate_interpolator(Args &&... args) - { - trajectory_->heading_rate_rps_ = std::make_shared>( - std::make_shared(std::forward(args)...)); - return *this; - } - - template - Builder & set_lane_ids_interpolator(Args &&... args) - { - trajectory_->lane_ids_ = std::make_shared>( - 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::trajectory - -#endif // AUTOWARE__TRAJECTORY__PATH_POINT_WITH_LANE_ID_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/point.hpp b/common/autoware_trajectory/include/autoware/trajectory/point.hpp deleted file mode 100644 index af47c404f7351..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/point.hpp +++ /dev/null @@ -1,162 +0,0 @@ -// 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__TRAJECTORY__POINT_HPP_ -#define AUTOWARE__TRAJECTORY__POINT_HPP_ - -#include "autoware/trajectory/forward.hpp" -#include "autoware/trajectory/interpolator/interpolator.hpp" - -#include - -#include - -#include -#include -#include -#include -#include - -namespace autoware::trajectory -{ -/** - * @brief Trajectory class for geometry_msgs::msg::Point - */ -template <> -class Trajectory -{ - 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 - - /** - * @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: - Trajectory(); - virtual ~Trajectory() = default; - Trajectory(const Trajectory & rhs); - Trajectory(Trajectory && rhs) = default; - Trajectory & operator=(const Trajectory & rhs); - Trajectory & operator=(Trajectory && rhs) = default; - - /** - * @brief Get the internal bases(arc lengths) of the trajectory - * @return Vector of bases(arc lengths) - */ - [[nodiscard]] virtual std::vector get_internal_bases() const; - /** - * @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 Restore the trajectory points - * @param min_points Minimum number of points - * @return Vector of points - */ - [[nodiscard]] std::vector restore(const size_t & min_points = 4) const; - - void crop(const double & start, const double & length); - - class Builder - { - private: - std::unique_ptr trajectory_; - - public: - Builder() : trajectory_(std::make_unique()) {} - - 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::trajectory - -#endif // AUTOWARE__TRAJECTORY__POINT_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/pose.hpp b/common/autoware_trajectory/include/autoware/trajectory/pose.hpp deleted file mode 100644 index 911db11bf8dfc..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/pose.hpp +++ /dev/null @@ -1,126 +0,0 @@ -// 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__TRAJECTORY__POSE_HPP_ -#define AUTOWARE__TRAJECTORY__POSE_HPP_ - -#include "autoware/trajectory/interpolator/interpolator.hpp" -#include "autoware/trajectory/point.hpp" - -#include -#include - -#include -#include -#include - -namespace autoware::trajectory -{ - -/** - * @brief Trajectory class for geometry_msgs::msg::Pose - */ -template <> -class Trajectory : public Trajectory -{ - using BaseClass = Trajectory; - using PointType = geometry_msgs::msg::Pose; - -protected: - std::shared_ptr> - orientation_interpolator_; //!< Interpolator for orientations - -public: - Trajectory(); - ~Trajectory() override = default; - Trajectory(const Trajectory & rhs); - Trajectory(Trajectory && rhs) = default; - Trajectory & operator=(const Trajectory & rhs); - Trajectory & operator=(Trajectory && rhs) = default; - - bool build(const std::vector & points); - - /** - * @brief Get the internal bases(arc lengths) of the trajectory - * @return Vector of bases(arc lengths) - */ - [[nodiscard]] std::vector get_internal_bases() const override; - - /** - * @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 = 4) 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()) {} - - 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::trajectory - -#endif // AUTOWARE__TRAJECTORY__POSE_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/closest.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/closest.hpp deleted file mode 100644 index 842e743271318..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/utils/closest.hpp +++ /dev/null @@ -1,100 +0,0 @@ -// 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__TRAJECTORY__UTILS__CLOSEST_HPP_ -#define AUTOWARE__TRAJECTORY__UTILS__CLOSEST_HPP_ - -#include "autoware/trajectory/detail/types.hpp" -#include "autoware/trajectory/forward.hpp" - -#include - -#include -#include -#include - -namespace autoware::trajectory -{ -namespace detail::impl -{ -/** - * @brief Internal implementation to find the closest point on a trajectory to a given point with - * constraints. - * @param trajectory_compute A function that computes a 2D point on the trajectory for a given - * parameter `s`. - * @param bases A vector of double values representing the sequence of bases for the trajectory. - * @param point The 2D point to which the closest point on the trajectory is to be found. - * @param constraint A function that evaluates whether a given parameter `s` satisfies the - * constraint. - * @return An optional double value representing the parameter `s` of the closest point on the - * trajectory that satisfies the constraint, or `std::nullopt` if no such point exists. - */ -std::optional closest_with_constraint_impl( - const std::function & trajectory_compute, - const std::vector & bases, const Eigen::Vector2d & point, - const std::function & constraint); -} // namespace detail::impl - -/** - * @brief Finds the closest point on a trajectory to a given point where the given constraint is - * satisfied. - * @tparam TrajectoryPointType The type of points in the trajectory. - * @tparam ArgPointType The type of the input point. - * @tparam Constraint A callable type that evaluates a constraint on a trajectory point. - * @param trajectory The trajectory to evaluate. - * @param point The point to which the closest point on the trajectory is to be found. - * @param constraint The constraint to apply to each point in the trajectory. - * @return An optional double value representing the parameter `s` of the closest point on the - * trajectory that satisfies the constraint, or `std::nullopt` if no such point exists. - */ -template -std::optional closest_with_constraint( - const trajectory::Trajectory & trajectory, const ArgPointType & point, - const Constraint & constraint) -{ - using autoware::trajectory::detail::to_point; - - return detail::impl::closest_with_constraint_impl( - [&trajectory](const double & s) { - TrajectoryPointType point = trajectory.compute(s); - Eigen::Vector2d result; - result << to_point(point).x, to_point(point).y; - return result; - }, - trajectory.get_internal_bases(), {to_point(point).x, to_point(point).y}, - [&constraint, &trajectory](const double & s) { return constraint(trajectory.compute(s)); }); -} - -/** - * @brief Finds the closest point on a trajectory to a given point. - * @tparam TrajectoryPointType The type of points in the trajectory. - * @tparam ArgPointType The type of the input point. - * @param trajectory The trajectory to evaluate. - * @param point The point to which the closest point on the trajectory is to be found. - * @return The parameter `s` of the closest point on the trajectory. - */ -template -double closest( - const trajectory::Trajectory & trajectory, const ArgPointType & point) -{ - std::optional s = - *closest_with_constraint(trajectory, point, [](const TrajectoryPointType &) { return true; }); - if (!s) { - throw std::runtime_error("No closest point found."); // This Exception should not be thrown. - } - return *s; -} -} // namespace autoware::trajectory - -#endif // AUTOWARE__TRAJECTORY__UTILS__CLOSEST_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/crop.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/crop.hpp deleted file mode 100644 index 1ce75c67d8128..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/utils/crop.hpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright 2025 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__TRAJECTORY__UTILS__CROP_HPP_ -#define AUTOWARE__TRAJECTORY__UTILS__CROP_HPP_ - -#include "autoware/trajectory/forward.hpp" - -namespace autoware::trajectory -{ - -template -trajectory::Trajectory crop( - trajectory::Trajectory trajectory, const double & start, const double & end) -{ - trajectory.crop(start, end); - return trajectory; -} - -} // namespace autoware::trajectory - -#endif // AUTOWARE__TRAJECTORY__UTILS__CROP_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/crossed.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/crossed.hpp deleted file mode 100644 index cb3406218800e..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/utils/crossed.hpp +++ /dev/null @@ -1,118 +0,0 @@ -// 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__TRAJECTORY__UTILS__CROSSED_HPP_ -#define AUTOWARE__TRAJECTORY__UTILS__CROSSED_HPP_ -#include "autoware/trajectory/detail/types.hpp" -#include "autoware/trajectory/forward.hpp" - -#include - -#include -#include - -namespace autoware::trajectory -{ -namespace detail::impl -{ - -/** - * @brief Internal implementation to find intersections between a trajectory and a linestring with - * constraints. - * @param trajectory_compute A function that computes a 2D point on the trajectory for a given - * parameter `s`. - * @param bases A vector of double values representing the sequence of bases for the trajectory. - * @param linestring A vector of pairs representing the linestring as a sequence of 2D line - * segments. - * @param constraint A function that evaluates whether a given parameter `s` satisfies the - * constraint. - * @return A vector of double values representing the parameters `s` where the trajectory intersects - * the linestring and satisfies the constraint. - */ -std::vector crossed_with_constraint_impl( - const std::function & trajectory_compute, - const std::vector & bases, // - const std::vector> & linestring, - const std::function & constraint); -} // namespace detail::impl - -/** - * @brief Finds intersections between a trajectory and a linestring where the given constraint is - * satisfied. - * @tparam TrajectoryPointType The type of points in the trajectory. - * @tparam LineStringType The type of the linestring. - * @tparam Constraint A callable type that evaluates a constraint on a trajectory point. - * @param trajectory The trajectory to evaluate. - * @param linestring The linestring to intersect with the trajectory. - * @param constraint The constraint to apply to each point in the trajectory. - * @return A vector of double values representing the parameters `s` where the trajectory intersects - * the linestring and satisfies the constraint. - */ -template -[[nodiscard]] std::vector crossed_with_constraint( - const trajectory::Trajectory & trajectory, const LineStringType & linestring, - const Constraint & constraint) -{ - using autoware::trajectory::detail::to_point; - - std::function trajectory_compute = - [&trajectory](const double & s) { - TrajectoryPointType point = trajectory.compute(s); - Eigen::Vector2d result; - result << to_point(point).x, to_point(point).y; - return result; - }; - - std::vector> linestring_eigen; - - if (linestring.end() - linestring.begin() < 2) { - return {}; - } - - auto point_it = linestring.begin(); - auto point_it_next = linestring.begin() + 1; - - for (; point_it_next != linestring.end(); ++point_it, ++point_it_next) { - Eigen::Vector2d start; - Eigen::Vector2d end; - start << point_it->x(), point_it->y(); - end << point_it_next->x(), point_it_next->y(); - linestring_eigen.emplace_back(start, end); - } - - return detail::impl::crossed_with_constraint_impl( - trajectory_compute, trajectory.get_internal_bases(), linestring_eigen, - [&constraint, &trajectory](const double & s) { return constraint(trajectory.compute(s)); }); -} - -/** - * @brief Finds intersections between a trajectory and a linestring. - * @tparam TrajectoryPointType The type of points in the trajectory. - * @tparam LineStringType The type of the linestring. - * @param trajectory The trajectory to evaluate. - * @param linestring The linestring to intersect with the trajectory. - * @return A vector of double values representing the parameters `s` where the trajectory intersects - * the linestring. - */ -template -[[nodiscard]] std::vector crossed( - const trajectory::Trajectory & trajectory, const LineStringType & linestring) -{ - return crossed_with_constraint( - trajectory, linestring, [](const TrajectoryPointType &) { return true; }); -} - -} // namespace autoware::trajectory - -#endif // AUTOWARE__TRAJECTORY__UTILS__CROSSED_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/find_intervals.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/find_intervals.hpp deleted file mode 100644 index ad816d0cd7917..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/utils/find_intervals.hpp +++ /dev/null @@ -1,76 +0,0 @@ -// 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__TRAJECTORY__UTILS__FIND_INTERVALS_HPP_ -#define AUTOWARE__TRAJECTORY__UTILS__FIND_INTERVALS_HPP_ - -#include "autoware/trajectory/detail/types.hpp" -#include "autoware/trajectory/forward.hpp" - -#include - -#include -#include -namespace autoware::trajectory -{ - -/** - * @struct Interval - * @brief Represents an interval with a start and end value. - */ -struct Interval -{ - double start; ///< Start value of the interval. - double end; ///< End value of the interval. -}; - -namespace detail::impl -{ - -/** - * @brief Internal implementation to find intervals in a sequence of bases that satisfy a - * constraint. - * @param bases A vector of double values representing the sequence of bases. - * @param constraint A function that evaluates whether a given base satisfies the constraint. - * @return A vector of Interval objects representing the intervals where the constraint is - * satisfied. - */ -std::vector find_intervals_impl( - const std::vector & bases, const std::function & constraint); - -} // namespace detail::impl - -/** - * @brief Finds intervals in a trajectory where the given constraint is satisfied. - * @tparam TrajectoryPointType The type of points in the trajectory. - * @tparam Constraint A callable type that evaluates a constraint on a trajectory point. - * @param trajectory The trajectory to evaluate. - * @param constraint The constraint to apply to each point in the trajectory. - * @return A vector of Interval objects representing the intervals where the constraint is - * satisfied. - */ -template -std::vector find_intervals( - const Trajectory & trajectory, const Constraint & constraint) -{ - using autoware::trajectory::detail::to_point; - - return detail::impl::find_intervals_impl( - trajectory.get_internal_bases(), - [&constraint, &trajectory](const double & s) { return constraint(trajectory.compute(s)); }); -} - -} // namespace autoware::trajectory - -#endif // AUTOWARE__TRAJECTORY__UTILS__FIND_INTERVALS_HPP_ diff --git a/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp b/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp deleted file mode 100644 index 1fa7f2401d31d..0000000000000 --- a/common/autoware_trajectory/include/autoware/trajectory/utils/shift.hpp +++ /dev/null @@ -1,119 +0,0 @@ -// 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__TRAJECTORY__UTILS__SHIFT_HPP_ -#define AUTOWARE__TRAJECTORY__UTILS__SHIFT_HPP_ - -#include "autoware/trajectory/detail/types.hpp" -#include "autoware/trajectory/forward.hpp" - -#include - -namespace autoware::trajectory -{ - -/** - * @struct ShiftInterval - * @brief Represents an interval for shifting a trajectory. - */ -struct ShiftInterval -{ - double start{0.0}; ///< Start position of the shift interval. - double end{0.0}; ///< End position of the shift interval. - double lateral_offset{0.0}; ///< Length of the shift to be applied. -}; - -/** - * @struct ShiftParameters - * @brief Represents parameters for shifting a trajectory. - */ -struct ShiftParameters -{ - double velocity{0.0}; ///< Velocity parameter for the shift. - double longitudinal_acc{0.0}; ///< Longitudinal acceleration parameter for the shift. - double lateral_acc_limit{-1.0}; ///< Lateral acceleration limit for the shift. -}; - -namespace detail::impl -{ - -/** - * @brief Internal implementation to apply a shift to a trajectory. - * @param bases A vector of double values representing the sequence of bases for the trajectory. - * @param shift_lengths A pointer to a vector of double values representing the shift lengths to be - * applied. - * @param shift_interval The interval over which the shift is applied. - * @param shift_parameters The parameters for the shift. - */ -void shift_impl( - const std::vector & bases, std::vector * shift_lengths, - const ShiftInterval & shift_interval, const ShiftParameters & shift_parameters); - -} // namespace detail::impl - -/** - * @brief Shifts a trajectory based on the provided shift intervals and parameters. - * @tparam PointType The type of points in the trajectory. - * @param reference_trajectory The reference trajectory to be shifted. - * @param shift_intervals A vector of ShiftInterval objects representing the intervals for shifting. - * @param shift_parameters The parameters for the shift. - * @return The shifted trajectory. - */ -template -trajectory::Trajectory shift( - const trajectory::Trajectory & reference_trajectory, - const std::vector & shift_intervals, const ShiftParameters & shift_parameters = {}) -{ - auto bases = reference_trajectory.get_internal_bases(); - std::vector shift_lengths(bases.size(), 0.0); - for (const auto & shift_interval : shift_intervals) { - detail::impl::shift_impl(bases, &shift_lengths, shift_interval, shift_parameters); - } - // Apply shift. - std::vector shifted_points; - for (size_t i = 0; i < bases.size(); ++i) { - shifted_points.emplace_back(reference_trajectory.compute(bases.at(i))); - double azimuth = reference_trajectory.azimuth(bases.at(i)); - const double shift_length = shift_lengths.at(i); - detail::to_point(shifted_points.back()).x += std::sin(azimuth) * shift_length; - detail::to_point(shifted_points.back()).y -= std::cos(azimuth) * shift_length; - } - auto shifted_trajectory = reference_trajectory; - const bool valid = shifted_trajectory.build(shifted_points); - if (!valid) { - throw std::runtime_error( - "Failed to build shifted trajectory"); // This Exception should not be thrown. - } - return shifted_trajectory; -} - -/** - * @brief Shifts a trajectory based on a single shift interval and parameters. - * @tparam PointType The type of points in the trajectory. - * @param reference_trajectory The reference trajectory to be shifted. - * @param shift_interval The interval for shifting. - * @param shift_parameters The parameters for the shift. - * @return The shifted trajectory. - */ -template -trajectory::Trajectory shift( - const trajectory::Trajectory & reference_trajectory, - const ShiftInterval & shift_interval, const ShiftParameters & shift_parameters = {}) -{ - return shift(reference_trajectory, std::vector{shift_interval}, shift_parameters); -} - -} // namespace autoware::trajectory - -#endif // AUTOWARE__TRAJECTORY__UTILS__SHIFT_HPP_ diff --git a/common/autoware_trajectory/package.xml b/common/autoware_trajectory/package.xml deleted file mode 100644 index bf935724684bc..0000000000000 --- a/common/autoware_trajectory/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - autoware_trajectory - 0.41.0 - The autoware_trajectory package - Yukinari Hisaki - Takayuki Murooka - Mamoru Sobue - Apache License 2.0 - - Yukinari Hisaki - - ament_cmake_auto - autoware_cmake - - autoware_internal_planning_msgs - autoware_planning_msgs - geometry_msgs - lanelet2_core - rclcpp - tf2 - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/autoware_trajectory/src/detail/logging.cpp b/common/autoware_trajectory/src/detail/logging.cpp deleted file mode 100644 index 3656a38fcc50a..0000000000000 --- a/common/autoware_trajectory/src/detail/logging.cpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright 2025 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/trajectory/detail/logging.hpp" - -namespace autoware::trajectory -{ - -rclcpp::Logger & get_logger() -{ - static rclcpp::Logger logger = rclcpp::get_logger("autoware_trajectory"); - return logger; -} - -rclcpp::Clock & get_clock() -{ - static rclcpp::Clock clock(RCL_ROS_TIME); - return clock; -} - -} // namespace autoware::trajectory diff --git a/common/autoware_trajectory/src/detail/types.cpp b/common/autoware_trajectory/src/detail/types.cpp deleted file mode 100644 index 9c3174f608a1a..0000000000000 --- a/common/autoware_trajectory/src/detail/types.cpp +++ /dev/null @@ -1,68 +0,0 @@ -// 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/trajectory/detail/types.hpp" - -namespace autoware::trajectory::detail -{ -MutablePoint3d to_point(geometry_msgs::msg::Point & p) -{ - return {p.x, p.y, p.z}; -} - -MutablePoint3d to_point(geometry_msgs::msg::Pose & p) -{ - return {p.position.x, p.position.y, p.position.z}; -} - -MutablePoint3d to_point(autoware_planning_msgs::msg::PathPoint & p) -{ - return {p.pose.position.x, p.pose.position.y, p.pose.position.z}; -} - -MutablePoint3d to_point(autoware_internal_planning_msgs::msg::PathPointWithLaneId & p) -{ - return {p.point.pose.position.x, p.point.pose.position.y, p.point.pose.position.z}; -} - -MutablePoint2d to_point(lanelet::BasicPoint2d & p) -{ - return {p.x(), p.y()}; -} - -ImmutablePoint3d to_point(const geometry_msgs::msg::Point & p) -{ - return {p.x, p.y, p.z}; -} - -ImmutablePoint3d to_point(const geometry_msgs::msg::Pose & p) -{ - return {p.position.x, p.position.y, p.position.z}; -} - -ImmutablePoint3d to_point(const autoware_planning_msgs::msg::PathPoint & p) -{ - return {p.pose.position.x, p.pose.position.y, p.pose.position.z}; -} - -ImmutablePoint3d to_point(const autoware_internal_planning_msgs::msg::PathPointWithLaneId & p) -{ - return {p.point.pose.position.x, p.point.pose.position.y, p.point.pose.position.z}; -} - -ImmutablePoint2d to_point(const lanelet::BasicPoint2d & p) -{ - return {p.x(), p.y()}; -} -} // namespace autoware::trajectory::detail diff --git a/common/autoware_trajectory/src/detail/util.cpp b/common/autoware_trajectory/src/detail/util.cpp deleted file mode 100644 index 58b54cb5ef76b..0000000000000 --- a/common/autoware_trajectory/src/detail/util.cpp +++ /dev/null @@ -1,83 +0,0 @@ -// 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/trajectory/detail/helpers.hpp" - -#include -#include - -namespace autoware::trajectory::detail -{ -inline namespace helpers -{ -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 (const 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 helpers -} // namespace autoware::trajectory::detail diff --git a/common/autoware_trajectory/src/interpolator/akima_spline.cpp b/common/autoware_trajectory/src/interpolator/akima_spline.cpp deleted file mode 100644 index 766d01ca85bbb..0000000000000 --- a/common/autoware_trajectory/src/interpolator/akima_spline.cpp +++ /dev/null @@ -1,92 +0,0 @@ -// 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/trajectory/interpolator/akima_spline.hpp" - -#include - -#include -#include - -namespace autoware::trajectory::interpolator -{ - -void AkimaSpline::compute_parameters( - const Eigen::Ref & bases, const Eigen::Ref & values) -{ - const auto n = static_cast(bases.size()); - - Eigen::VectorXd h = bases.tail(n - 1) - bases.head(n - 1); - - Eigen::VectorXd m(n - 1); - for (int32_t i = 0; i < n - 1; ++i) { - m[i] = (values[i + 1] - values[i]) / h[i]; - } - - Eigen::VectorXd s(n); - s[0] = m[0]; - s[1] = (m[0] + m[1]) / 2; - for (int32_t i = 2; i < n - 2; ++i) { - if ((std::abs(m[i + 1] - m[i]) + std::abs(m[i - 1] - m[i - 2])) == 0) { - s[i] = (m[i] + m[i - 1]) / 2; - } else { - s[i] = (std::abs(m[i + 1] - m[i]) * m[i - 1] + std::abs(m[i - 1] - m[i - 2]) * m[i]) / - (std::abs(m[i + 1] - m[i]) + std::abs(m[i - 1] - m[i - 2])); - } - } - s[n - 2] = (m[n - 2] + m[n - 3]) / 2; - s[n - 1] = m[n - 2]; - - a_.resize(n - 1); - b_.resize(n - 1); - c_.resize(n - 1); - d_.resize(n - 1); - for (int32_t i = 0; i < n - 1; ++i) { - a_[i] = values[i]; - b_[i] = s[i]; - c_[i] = (3 * m[i] - 2 * s[i] - s[i + 1]) / h[i]; - d_[i] = (s[i] + s[i + 1] - 2 * m[i]) / (h[i] * h[i]); - } -} - -void AkimaSpline::build_impl(const std::vector & bases, const std::vector & values) -{ - this->bases_ = bases; - compute_parameters( - Eigen::Map(bases.data(), static_cast(bases.size())), - Eigen::Map(values.data(), static_cast(values.size()))); -} - -double AkimaSpline::compute_impl(const double & s) const -{ - const int32_t i = this->get_index(s); - const double dx = s - this->bases_[i]; - return a_[i] + b_[i] * dx + c_[i] * dx * dx + d_[i] * dx * dx * dx; -} - -double AkimaSpline::compute_first_derivative_impl(const double & s) const -{ - const int32_t i = this->get_index(s); - const double dx = s - this->bases_[i]; - return b_[i] + 2 * c_[i] * dx + 3 * d_[i] * dx * dx; -} - -double AkimaSpline::compute_second_derivative_impl(const double & s) const -{ - const int32_t i = this->get_index(s); - const double dx = s - this->bases_[i]; - return 2 * c_[i] + 6 * d_[i] * dx; -} - -} // namespace autoware::trajectory::interpolator diff --git a/common/autoware_trajectory/src/interpolator/cubic_spline.cpp b/common/autoware_trajectory/src/interpolator/cubic_spline.cpp deleted file mode 100644 index 378233e799902..0000000000000 --- a/common/autoware_trajectory/src/interpolator/cubic_spline.cpp +++ /dev/null @@ -1,93 +0,0 @@ -// 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/trajectory/interpolator/cubic_spline.hpp" - -#include - -namespace autoware::trajectory::interpolator -{ - -void CubicSpline::compute_parameters( - const Eigen::Ref & bases, const Eigen::Ref & values) -{ - const int32_t n = static_cast(bases.size()) - 1; - - h_ = bases.tail(n) - bases.head(n); - a_ = values.transpose(); - - for (int32_t i = 0; i < n; ++i) { - h_(i) = bases(i + 1) - bases(i); - } - - Eigen::VectorXd alpha(n - 1); - for (int32_t i = 1; i < n; ++i) { - alpha(i - 1) = (3.0 / h_(i)) * (a_(i + 1) - a_(i)) - (3.0 / h_(i - 1)) * (a_(i) - a_(i - 1)); - } - - Eigen::VectorXd l(n + 1); - Eigen::VectorXd mu(n + 1); - Eigen::VectorXd z(n + 1); - l(0) = 1.0; - mu(0) = z(0) = 0.0; - - for (int32_t i = 1; i < n; ++i) { - l(i) = 2.0 * (bases(i + 1) - bases(i - 1)) - h_(i - 1) * mu(i - 1); - mu(i) = h_(i) / l(i); - z(i) = (alpha(i - 1) - h_(i - 1) * z(i - 1)) / l(i); - } - b_.resize(n); - d_.resize(n); - c_.resize(n + 1); - - l(n) = 1.0; - z(n) = c_(n) = 0.0; - - for (int32_t j = n - 1; j >= 0; --j) { - c_(j) = z(j) - mu(j) * c_(j + 1); - b_(j) = (a_(j + 1) - a_(j)) / h_(j) - h_(j) * (c_(j + 1) + 2.0 * c_(j)) / 3.0; - d_(j) = (c_(j + 1) - c_(j)) / (3.0 * h_(j)); - } -} - -void CubicSpline::build_impl(const std::vector & bases, const std::vector & values) -{ - this->bases_ = bases; - compute_parameters( - Eigen::Map(bases.data(), static_cast(bases.size())), - Eigen::Map(values.data(), static_cast(values.size()))); -} - -double CubicSpline::compute_impl(const double & s) const -{ - const int32_t i = this->get_index(s); - const double dx = s - this->bases_.at(i); - return a_(i) + b_(i) * dx + c_(i) * dx * dx + d_(i) * dx * dx * dx; -} - -double CubicSpline::compute_first_derivative_impl(const double & s) const -{ - const int32_t i = this->get_index(s); - const double dx = s - this->bases_.at(i); - return b_(i) + 2 * c_(i) * dx + 3 * d_(i) * dx * dx; -} - -double CubicSpline::compute_second_derivative_impl(const double & s) const -{ - const int32_t i = this->get_index(s); - const double dx = s - this->bases_.at(i); - return 2 * c_(i) + 6 * d_(i) * dx; -} - -} // namespace autoware::trajectory::interpolator diff --git a/common/autoware_trajectory/src/interpolator/linear.cpp b/common/autoware_trajectory/src/interpolator/linear.cpp deleted file mode 100644 index 7d75b759050b8..0000000000000 --- a/common/autoware_trajectory/src/interpolator/linear.cpp +++ /dev/null @@ -1,60 +0,0 @@ -// 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/trajectory/interpolator/linear.hpp" - -#include - -#include - -namespace autoware::trajectory::interpolator -{ - -void Linear::build_impl(const std::vector & bases, const std::vector & values) -{ - this->bases_ = bases; - this->values_ = - Eigen::Map(values.data(), static_cast(values.size())); -} - -double Linear::compute_impl(const double & s) const -{ - const int32_t idx = this->get_index(s); - const double x0 = this->bases_.at(idx); - const double x1 = this->bases_.at(idx + 1); - const double y0 = this->values_(idx); - const double y1 = this->values_(idx + 1); - return y0 + (y1 - y0) * (s - x0) / (x1 - x0); -} - -double Linear::compute_first_derivative_impl(const double & s) const -{ - const int32_t idx = this->get_index(s); - const double x0 = this->bases_.at(idx); - const double x1 = this->bases_.at(idx + 1); - const double y0 = this->values_(idx); - const double y1 = this->values_(idx + 1); - return (y1 - y0) / (x1 - x0); -} - -double Linear::compute_second_derivative_impl(const double &) const -{ - return 0.0; -} - -size_t Linear::minimum_required_points() const -{ - return 2; -} -} // namespace autoware::trajectory::interpolator diff --git a/common/autoware_trajectory/src/interpolator/spherical_linear.cpp b/common/autoware_trajectory/src/interpolator/spherical_linear.cpp deleted file mode 100644 index f255135ae898b..0000000000000 --- a/common/autoware_trajectory/src/interpolator/spherical_linear.cpp +++ /dev/null @@ -1,64 +0,0 @@ -// 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/trajectory/interpolator/spherical_linear.hpp" - -#include - -#include - -namespace autoware::trajectory::interpolator -{ - -void SphericalLinear::build_impl( - const std::vector & bases, - const std::vector & quaternions) -{ - this->bases_ = bases; - this->quaternions_ = quaternions; -} - -geometry_msgs::msg::Quaternion SphericalLinear::compute_impl(const double & s) const -{ - const int32_t idx = this->get_index(s); - const double x0 = this->bases_.at(idx); - const double x1 = this->bases_.at(idx + 1); - const geometry_msgs::msg::Quaternion y0 = this->quaternions_.at(idx); - const geometry_msgs::msg::Quaternion y1 = this->quaternions_.at(idx + 1); - - // Spherical linear interpolation (Slerp) - const double t = (s - x0) / (x1 - x0); - - // Convert quaternions to Eigen vectors for calculation - const Eigen::Quaterniond q0(y0.w, y0.x, y0.y, y0.z); - const Eigen::Quaterniond q1(y1.w, y1.x, y1.y, y1.z); - - // Perform Slerp - Eigen::Quaterniond q_slerp = q0.slerp(t, q1); - - // Convert the result back to geometry_msgs::msg::Quaternion - geometry_msgs::msg::Quaternion result; - result.w = q_slerp.w(); - result.x = q_slerp.x(); - result.y = q_slerp.y(); - result.z = q_slerp.z(); - - return result; -} - -size_t SphericalLinear::minimum_required_points() const -{ - return 2; -} -} // namespace autoware::trajectory::interpolator diff --git a/common/autoware_trajectory/src/path_point.cpp b/common/autoware_trajectory/src/path_point.cpp deleted file mode 100644 index d264d1d054d87..0000000000000 --- a/common/autoware_trajectory/src/path_point.cpp +++ /dev/null @@ -1,130 +0,0 @@ -// 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/trajectory/path_point.hpp" - -#include "autoware/trajectory/detail/helpers.hpp" -#include "autoware/trajectory/detail/interpolated_array.hpp" -#include "autoware/trajectory/forward.hpp" -#include "autoware/trajectory/interpolator/stairstep.hpp" -#include "autoware/trajectory/pose.hpp" - -#include - -#include -#include - -namespace autoware::trajectory -{ - -using PointType = autoware_planning_msgs::msg::PathPoint; - -Trajectory::Trajectory() -: longitudinal_velocity_mps_(std::make_shared>( - std::make_shared>())), - lateral_velocity_mps_(std::make_shared>( - std::make_shared>())), - heading_rate_rps_(std::make_shared>( - std::make_shared>())) -{ -} - -Trajectory::Trajectory(const Trajectory & rhs) -: BaseClass(rhs), - longitudinal_velocity_mps_( - std::make_shared>(*rhs.longitudinal_velocity_mps_)), - lateral_velocity_mps_( - std::make_shared>(*rhs.lateral_velocity_mps_)), - heading_rate_rps_(std::make_shared>(*rhs.heading_rate_rps_)) -{ -} - -Trajectory & Trajectory::operator=(const Trajectory & rhs) -{ - if (this != &rhs) { - BaseClass::operator=(rhs); - *longitudinal_velocity_mps_ = *rhs.longitudinal_velocity_mps_; - *lateral_velocity_mps_ = *rhs.lateral_velocity_mps_; - *heading_rate_rps_ = *rhs.heading_rate_rps_; - } - return *this; -} - -bool Trajectory::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 &= Trajectory::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; -} - -std::vector Trajectory::get_internal_bases() 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_); - std::transform( - bases.begin(), bases.end(), bases.begin(), [this](const double & s) { return s - start_; }); - return bases; -} - -PointType Trajectory::compute(double s) const -{ - PointType result; - result.pose = Trajectory::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 Trajectory::restore(const size_t & min_points) const -{ - std::vector bases = get_internal_bases(); - bases = detail::fill_bases(bases, min_points); - - std::vector points; - points.reserve(bases.size()); - for (const auto & s : bases) { - points.emplace_back(compute(s)); - } - return points; -} - -} // namespace autoware::trajectory diff --git a/common/autoware_trajectory/src/path_point_with_lane_id.cpp b/common/autoware_trajectory/src/path_point_with_lane_id.cpp deleted file mode 100644 index 96d3cfd91a027..0000000000000 --- a/common/autoware_trajectory/src/path_point_with_lane_id.cpp +++ /dev/null @@ -1,98 +0,0 @@ -// 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/trajectory/path_point_with_lane_id.hpp" - -#include "autoware/trajectory/detail/helpers.hpp" -#include "autoware/trajectory/interpolator/stairstep.hpp" - -#include -#include - -namespace autoware::trajectory -{ - -using PointType = autoware_internal_planning_msgs::msg::PathPointWithLaneId; - -Trajectory::Trajectory() -: lane_ids_(std::make_shared>( - std::make_shared>())) -{ -} - -Trajectory & Trajectory::operator=(const Trajectory & rhs) -{ - if (this != &rhs) { - BaseClass::operator=(rhs); - lane_ids_ = std::make_shared>(this->lane_ids()); - } - return *this; -} - -bool Trajectory::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; -} - -std::vector Trajectory::get_internal_bases() 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_); - - std::transform( - bases.begin(), bases.end(), bases.begin(), [this](const double & s) { return s - start_; }); - return bases; -} - -PointType Trajectory::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 Trajectory::restore(const size_t & min_points) const -{ - auto bases = get_internal_bases(); - bases = detail::fill_bases(bases, min_points); - - std::vector points; - points.reserve(bases.size()); - for (const auto & s : bases) { - points.emplace_back(compute(s)); - } - return points; -} - -} // namespace autoware::trajectory diff --git a/common/autoware_trajectory/src/point.cpp b/common/autoware_trajectory/src/point.cpp deleted file mode 100644 index 405bc7e7a2733..0000000000000 --- a/common/autoware_trajectory/src/point.cpp +++ /dev/null @@ -1,173 +0,0 @@ -// 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/trajectory/point.hpp" - -#include "autoware/trajectory/detail/helpers.hpp" -#include "autoware/trajectory/interpolator/cubic_spline.hpp" -#include "autoware/trajectory/interpolator/linear.hpp" - -#include -#include - -#include -#include -#include -#include -#include - -namespace autoware::trajectory -{ - -using PointType = geometry_msgs::msg::Point; - -Trajectory::Trajectory() -: x_interpolator_(std::make_shared()), - y_interpolator_(std::make_shared()), - z_interpolator_(std::make_shared()) -{ -} - -Trajectory::Trajectory(const Trajectory & rhs) -: x_interpolator_(rhs.x_interpolator_->clone()), - y_interpolator_(rhs.y_interpolator_->clone()), - z_interpolator_(rhs.z_interpolator_->clone()), - bases_(rhs.bases_), - start_(rhs.start_), - end_(rhs.end_) -{ -} - -Trajectory & Trajectory::operator=(const Trajectory & rhs) -{ - if (this != &rhs) { - x_interpolator_ = rhs.x_interpolator_->clone(); - y_interpolator_ = rhs.y_interpolator_->clone(); - z_interpolator_ = rhs.z_interpolator_->clone(); - bases_ = rhs.bases_; - start_ = rhs.start_; - end_ = rhs.end_; - } - return *this; -} - -bool Trajectory::build(const std::vector & points) -{ - std::vector xs; - std::vector ys; - std::vector zs; - - bases_.clear(); - 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) { - const Eigen::Vector2d p0(points[i - 1].x, points[i - 1].y); - const 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 Trajectory::clamp(const double & s, bool show_warning) const -{ - if ((s < 0 || s > length()) && show_warning) { - RCLCPP_WARN( - rclcpp::get_logger("Trajectory"), "The arc length %f is out of the trajectory length %f", s, - length()); - } - return std::clamp(s, 0.0, length()) + start_; -} - -std::vector Trajectory::get_internal_bases() const -{ - auto bases = detail::crop_bases(bases_, start_, end_); - std::transform( - bases.begin(), bases.end(), bases.begin(), [this](const double & s) { return s - start_; }); - return bases; -} - -double Trajectory::length() const -{ - return end_ - start_; -} - -PointType Trajectory::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 Trajectory::azimuth(double s) const -{ - s = clamp(s, true); - const double dx = x_interpolator_->compute_first_derivative(s); - const double dy = y_interpolator_->compute_first_derivative(s); - return std::atan2(dy, dx); -} - -double Trajectory::elevation(double s) const -{ - s = clamp(s, true); - const double dz = z_interpolator_->compute_first_derivative(s); - return std::atan2(dz, 1.0); -} - -double Trajectory::curvature(double s) const -{ - s = clamp(s, true); - const double dx = x_interpolator_->compute_first_derivative(s); - const double ddx = x_interpolator_->compute_second_derivative(s); - const double dy = y_interpolator_->compute_first_derivative(s); - const 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 Trajectory::restore(const size_t & min_points) const -{ - auto bases = get_internal_bases(); - bases = detail::fill_bases(bases, min_points); - std::vector points; - points.reserve(bases.size()); - for (const auto & s : bases) { - points.emplace_back(compute(s)); - } - return points; -} - -void Trajectory::crop(const double & start, const double & length) -{ - start_ = std::clamp(start_ + start, start_, end_); - end_ = std::clamp(start_ + length, start_, end_); -} - -} // namespace autoware::trajectory diff --git a/common/autoware_trajectory/src/pose.cpp b/common/autoware_trajectory/src/pose.cpp deleted file mode 100644 index 0dc3287769aea..0000000000000 --- a/common/autoware_trajectory/src/pose.cpp +++ /dev/null @@ -1,134 +0,0 @@ -// 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/trajectory/pose.hpp" - -#include "autoware/trajectory/detail/helpers.hpp" -#include "autoware/trajectory/interpolator/spherical_linear.hpp" - -#include -#include - -#include - -namespace autoware::trajectory -{ -using PointType = geometry_msgs::msg::Pose; - -Trajectory::Trajectory() -: orientation_interpolator_(std::make_shared()) -{ -} - -Trajectory::Trajectory(const Trajectory & rhs) -: BaseClass(rhs), orientation_interpolator_(rhs.orientation_interpolator_->clone()) -{ -} - -Trajectory & Trajectory::operator=(const Trajectory & rhs) -{ - if (this != &rhs) { - BaseClass::operator=(rhs); - orientation_interpolator_ = rhs.orientation_interpolator_->clone(); - } - return *this; -} - -bool Trajectory::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; -} - -std::vector Trajectory::get_internal_bases() const -{ - auto bases = detail::crop_bases(bases_, start_, end_); - std::transform( - bases.begin(), bases.end(), bases.begin(), [this](const double & s) { return s - start_; }); - return bases; -} - -PointType Trajectory::compute(double s) const -{ - PointType result; - result.position = BaseClass::compute(s); - s = clamp(s); - result.orientation = orientation_interpolator_->compute(s); - return result; -} - -void Trajectory::align_orientation_with_trajectory_direction() -{ - std::vector aligned_orientations; - for (const auto & s : bases_) { - const double azimuth = this->azimuth(s); - const double elevation = this->elevation(s); - const 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(); - const tf2::Vector3 x_axis(1.0, 0.0, 0.0); - const tf2::Vector3 current_x_axis = tf2::quatRotate(current_orientation_tf2, x_axis); - - const tf2::Vector3 desired_x_axis( - std::cos(elevation) * std::cos(azimuth), std::cos(elevation) * std::sin(azimuth), - std::sin(elevation)); - const tf2::Vector3 rotation_axis = current_x_axis.cross(desired_x_axis).normalized(); - const double dot_product = current_x_axis.dot(desired_x_axis); - const double rotation_angle = std::acos(dot_product); - - const tf2::Quaternion delta_q(rotation_axis, rotation_angle); - const 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); - } - const bool success = orientation_interpolator_->build(bases_, aligned_orientations); - if (!success) { - throw std::runtime_error( - "Failed to build orientation interpolator."); // This Exception should not be thrown. - } -} - -std::vector Trajectory::restore(const size_t & min_points) const -{ - auto bases = get_internal_bases(); - bases = detail::fill_bases(bases, min_points); - std::vector points; - points.reserve(bases.size()); - for (const auto & s : bases) { - points.emplace_back(compute(s)); - } - return points; -} - -} // namespace autoware::trajectory diff --git a/common/autoware_trajectory/src/utils/closest.cpp b/common/autoware_trajectory/src/utils/closest.cpp deleted file mode 100644 index 76537f7ac122f..0000000000000 --- a/common/autoware_trajectory/src/utils/closest.cpp +++ /dev/null @@ -1,64 +0,0 @@ -// 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/trajectory/utils/closest.hpp" - -#include -#include - -namespace autoware::trajectory::detail::impl -{ -std::optional closest_with_constraint_impl( - const std::function & trajectory_compute, - const std::vector & bases, const Eigen::Vector2d & point, - const std::function & constraint) -{ - using trajectory::detail::to_point; - std::vector distances_from_segments; - std::vector lengths_from_start_points; - - for (size_t i = 1; i < bases.size(); ++i) { - const Eigen::Vector2d p0 = trajectory_compute(bases.at(i - 1)); - const Eigen::Vector2d p1 = trajectory_compute(bases.at(i)); - - const Eigen::Vector2d v = p1 - p0; - const Eigen::Vector2d w = point - p0; - const double c1 = w.dot(v); - const double c2 = v.dot(v); - double length_from_start_point = NAN; - double distance_from_segment = NAN; - if (c1 <= 0) { - length_from_start_point = bases.at(i - 1); - distance_from_segment = (point - p0).norm(); - } else if (c2 <= c1) { - length_from_start_point = bases.at(i); - distance_from_segment = (point - p1).norm(); - } else { - length_from_start_point = bases.at(i - 1) + c1 / c2 * (p1 - p0).norm(); - distance_from_segment = (point - (p0 + (c1 / c2) * v)).norm(); - } - if (constraint(length_from_start_point)) { - distances_from_segments.push_back(distance_from_segment); - lengths_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 lengths_from_start_points[std::distance(distances_from_segments.begin(), min_it)]; -} -} // namespace autoware::trajectory::detail::impl diff --git a/common/autoware_trajectory/src/utils/crossed.cpp b/common/autoware_trajectory/src/utils/crossed.cpp deleted file mode 100644 index f0052ee9e6164..0000000000000 --- a/common/autoware_trajectory/src/utils/crossed.cpp +++ /dev/null @@ -1,86 +0,0 @@ -// 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/trajectory/utils/crossed.hpp" - -#include -#include - -namespace autoware::trajectory::detail::impl -{ - -std::optional crossed_with_constraint_impl( - const std::function & trajectory_compute, - const std::vector & bases, // - const Eigen::Vector2d & line_start, const Eigen::Vector2d & line_end, - const std::function & constraint) -{ - Eigen::Vector2d line_dir = line_end - line_start; - - for (size_t i = 1; i < bases.size(); ++i) { - const Eigen::Vector2d p0 = trajectory_compute(bases.at(i - 1)); - const Eigen::Vector2d p1 = trajectory_compute(bases.at(i)); - - Eigen::Vector2d segment_dir = p1 - p0; - - const 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; - - const double t = - (p0_to_line_start.x() * line_dir.y() - p0_to_line_start.y() * line_dir.x()) / det; - const 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 = bases.at(i - 1) + t * (bases.at(i) - bases.at(i - 1)); - if (constraint(intersection)) { - return intersection; - } - } - } - - return std::nullopt; -} - -std::vector crossed_with_constraint_impl( - const std::function & trajectory_compute, - const std::vector & bases, // - const std::vector> & linestring, - const std::function & constraint) -{ - using trajectory::detail::to_point; - - std::vector intersections; - - for (const auto & line : linestring) { - const Eigen::Vector2d & line_start = line.first; - const Eigen::Vector2d & line_end = line.second; - - std::optional intersection = - crossed_with_constraint_impl(trajectory_compute, bases, line_start, line_end, constraint); - - if (intersection) { - intersections.push_back(*intersection); - } - } - - return intersections; -} - -} // namespace autoware::trajectory::detail::impl diff --git a/common/autoware_trajectory/src/utils/find_intervals.cpp b/common/autoware_trajectory/src/utils/find_intervals.cpp deleted file mode 100644 index 4899f07f9cccd..0000000000000 --- a/common/autoware_trajectory/src/utils/find_intervals.cpp +++ /dev/null @@ -1,42 +0,0 @@ -// 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/trajectory/utils/find_intervals.hpp" - -#include -#include -#include - -namespace autoware::trajectory::detail::impl -{ - -std::vector find_intervals_impl( - const std::vector & bases, const std::function & constraint) -{ - std::vector intervals; - - std::optional start = std::nullopt; - for (size_t i = 0; i < bases.size(); ++i) { - if (!start && constraint(bases.at(i))) { - start = bases.at(i); // Start a new interval - } else if (start && (!constraint(bases.at(i)) || i == bases.size() - 1)) { - // End the current interval if the constraint fails or it's the last element - intervals.emplace_back(Interval{start.value(), bases.at(i - !constraint(bases.at(i)))}); - start = std::nullopt; // Reset the start - } - } - return intervals; -} - -} // namespace autoware::trajectory::detail::impl diff --git a/common/autoware_trajectory/src/utils/shift.cpp b/common/autoware_trajectory/src/utils/shift.cpp deleted file mode 100644 index f12e8df11f1f0..0000000000000 --- a/common/autoware_trajectory/src/utils/shift.cpp +++ /dev/null @@ -1,280 +0,0 @@ -// 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/trajectory/utils/shift.hpp" - -#include "autoware/trajectory/detail/logging.hpp" -#include "autoware/trajectory/interpolator/cubic_spline.hpp" - -#include -#include -#include -#include - -namespace autoware::trajectory::detail::impl -{ - -// This function calculates base longitudinal and lateral lengths -// when acceleration limit is not considered (simple division approach). -std::pair, std::vector> get_base_lengths_without_accel_limit( - const double arc_length, const double shift_length) -{ - // Alias for clarity - const double total_arc_length = arc_length; - const double total_shift_length = shift_length; - - // Prepare base longitudinal positions - const std::vector base_longitudinal = { - 0.0, 0.25 * total_arc_length, 0.75 * total_arc_length, total_arc_length}; - - // Prepare base lateral positions - const std::vector base_lateral = { - 0.0, 1.0 / 12.0 * total_shift_length, 11.0 / 12.0 * total_shift_length, total_shift_length}; - - return {base_longitudinal, base_lateral}; -} - -// This function calculates base longitudinal and lateral lengths -// when acceleration limit is not considered, but velocity and acceleration are known. -std::pair, std::vector> get_base_lengths_without_accel_limit( - const double arc_length, const double shift_length, const double velocity, - const double longitudinal_acc, const double total_time) -{ - // Aliases for clarity - const double total_arc_length = arc_length; - const double total_shift_length = shift_length; - const double v0 = velocity; // initial velocity - const double a = longitudinal_acc; // longitudinal acceleration - const double t = total_time / 4.0; // quarter of total time - - // Calculate first segment in longitudinal direction - // s1 = v0 * t + 1/2 * a * t^2 (but capped by total_arc_length) - const double segment_s1 = std::min(v0 * t + 0.5 * a * t * t, total_arc_length); - - // Velocity at the end of first segment - const double v1 = v0 + a * t; - - // Calculate second segment in longitudinal direction - // s2 = s1 + 2 * v1 * t + 2 * a * t^2 (but capped by total_arc_length) - const double segment_s2 = std::min(segment_s1 + 2.0 * v1 * t + 2.0 * a * t * t, total_arc_length); - - // Prepare base longitudinal positions - const std::vector base_longitudinal = {0.0, segment_s1, segment_s2, total_arc_length}; - - // Prepare base lateral positions (simple division approach as original) - const std::vector base_lateral = { - 0.0, 1.0 / 12.0 * total_shift_length, 11.0 / 12.0 * total_shift_length, total_shift_length}; - - return {base_longitudinal, base_lateral}; -} -std::pair, std::vector> calc_base_lengths( - const double & arc_length, const double & shift_length, const ShiftParameters & shift_parameters) -{ - // Threshold for treating acceleration as zero - const double acc_threshold = 1.0e-4; - - // Extract initial velocity and clamp negative acceleration to zero - const double initial_vel = std::abs(shift_parameters.velocity); - const double used_lon_acc = - (shift_parameters.longitudinal_acc > acc_threshold) ? shift_parameters.longitudinal_acc : 0.0; - - // If there is no need to consider acceleration limit - if (initial_vel < 1.0e-5 && used_lon_acc < acc_threshold) { - RCLCPP_DEBUG( - get_logger(), - "Velocity is effectively zero. " - "No lateral acceleration limit will be applied."); - return get_base_lengths_without_accel_limit(arc_length, shift_length); - } - - // Prepare main parameters - const double target_arclength = arc_length; - const double target_shift_abs = std::abs(shift_length); - - // Calculate total time (total_time) to travel 'target_arclength' - // If used_lon_acc is valid (> 0), use time from kinematic formula. Otherwise, use s/v - const double total_time = - (used_lon_acc > acc_threshold) - ? (-initial_vel + - std::sqrt(initial_vel * initial_vel + 2.0 * used_lon_acc * target_arclength)) / - used_lon_acc - : (target_arclength / initial_vel); - - // Calculate the maximum lateral acceleration if we do not add further constraints - const double max_lateral_acc = 8.0 * target_shift_abs / (total_time * total_time); - - // If the max_lateral_acc is already below the limit, no need to reduce it - if (max_lateral_acc < shift_parameters.lateral_acc_limit) { - RCLCPP_DEBUG_THROTTLE( - get_logger(), get_clock(), 3000, "No need to consider lateral acc limit. max: %f, limit: %f", - max_lateral_acc, shift_parameters.lateral_acc_limit); - return get_base_lengths_without_accel_limit( - target_arclength, shift_length, initial_vel, used_lon_acc, total_time); - } - - // Compute intermediate times (jerk_time / accel_time) and lateral jerk - const double jerk_time = - total_time / 2.0 - 2.0 * target_shift_abs / (shift_parameters.lateral_acc_limit * total_time); - const double accel_time = - 4.0 * target_shift_abs / (shift_parameters.lateral_acc_limit * total_time) - total_time / 2.0; - const double lateral_jerk = - (2.0 * shift_parameters.lateral_acc_limit * shift_parameters.lateral_acc_limit * total_time) / - (shift_parameters.lateral_acc_limit * total_time * total_time - 4.0 * target_shift_abs); - - // If computed times or jerk are invalid (negative or too small), skip the acc limit - if (jerk_time < 0.0 || accel_time < 0.0 || lateral_jerk < 0.0 || (jerk_time / total_time) < 0.1) { - RCLCPP_WARN_THROTTLE( - get_logger(), get_clock(), 3000, - "The specified lateral acceleration limit appears too restrictive. " - "No feasible jerk_time or accel_time was found. " - "Possible reasons: (1) shift_length is too large, (2) lateral_acc_limit is too low, " - "or (3) system kinematics are outside valid range.\n" - "Details:\n" - " - jerk_time: %.4f\n" - " - accel_time: %.4f\n" - " - lateral_jerk: %.4f\n" - " - computed_lateral_acc: %.4f\n" - " - lateral_acc_limit: %.4f\n\n" - "Suggestions:\n" - " - Increase the lateral_acc_limit.\n" - " - Decrease shift_length if possible.\n" - " - Re-check velocity and acceleration settings for consistency.", - jerk_time, accel_time, lateral_jerk, max_lateral_acc, shift_parameters.lateral_acc_limit); - return get_base_lengths_without_accel_limit(target_arclength, shift_length); - } - - // Precompute powers for jerk_time and accel_time - const double jerk_time3 = jerk_time * jerk_time * jerk_time; - const double accel_time2_jerk = accel_time * accel_time * jerk_time; - const double accel_time_jerk2 = accel_time * jerk_time * jerk_time; - - // ------------------------------------------------------ - // Calculate longitudinal base points - // ------------------------------------------------------ - // Segment s1 - const double segment_s1 = std::min( - jerk_time * initial_vel + 0.5 * used_lon_acc * jerk_time * jerk_time, target_arclength); - const double v1 = initial_vel + used_lon_acc * jerk_time; - - // Segment s2 - const double segment_s2 = std::min( - segment_s1 + accel_time * v1 + 0.5 * used_lon_acc * accel_time * accel_time, target_arclength); - const double v2 = v1 + used_lon_acc * accel_time; - - // Segment s3 = s4 - const double segment_s3 = std::min( - segment_s2 + jerk_time * v2 + 0.5 * used_lon_acc * jerk_time * jerk_time, target_arclength); - const double v3 = v2 + used_lon_acc * jerk_time; - - // Segment s5 - const double segment_s5 = std::min( - segment_s3 + jerk_time * v3 + 0.5 * used_lon_acc * jerk_time * jerk_time, target_arclength); - const double v5 = v3 + used_lon_acc * jerk_time; - - // Segment s6 - const double segment_s6 = std::min( - segment_s5 + accel_time * v5 + 0.5 * used_lon_acc * accel_time * accel_time, target_arclength); - const double v6 = v5 + used_lon_acc * accel_time; - - // Segment s7 - const double segment_s7 = std::min( - segment_s6 + jerk_time * v6 + 0.5 * used_lon_acc * jerk_time * jerk_time, target_arclength); - - // ------------------------------------------------------ - // Calculate lateral base points - // ------------------------------------------------------ - // sign determines the direction of shift - const double shift_sign = (shift_length > 0.0) ? 1.0 : -1.0; - - // Shift amounts at each segment - const double shift_l1 = shift_sign * (1.0 / 6.0 * lateral_jerk * jerk_time3); - const double shift_l2 = - shift_sign * (1.0 / 6.0 * lateral_jerk * jerk_time3 + 0.5 * lateral_jerk * accel_time_jerk2 + - 0.5 * lateral_jerk * accel_time2_jerk); - const double shift_l3 = - shift_sign * (lateral_jerk * jerk_time3 + 1.5 * lateral_jerk * accel_time_jerk2 + - 0.5 * lateral_jerk * accel_time2_jerk); // = shift_l4 - const double shift_l5 = - shift_sign * (11.0 / 6.0 * lateral_jerk * jerk_time3 + 2.5 * lateral_jerk * accel_time_jerk2 + - 0.5 * lateral_jerk * accel_time2_jerk); - const double shift_l6 = - shift_sign * (11.0 / 6.0 * lateral_jerk * jerk_time3 + 3.0 * lateral_jerk * accel_time_jerk2 + - lateral_jerk * accel_time2_jerk); - const double shift_l7 = - shift_sign * (2.0 * lateral_jerk * jerk_time3 + 3.0 * lateral_jerk * accel_time_jerk2 + - lateral_jerk * accel_time2_jerk); - - // Construct the output vectors - const std::vector base_lon = {0.0, segment_s1, segment_s2, segment_s3, - segment_s5, segment_s6, segment_s7}; - const std::vector base_lat = {0.0, shift_l1, shift_l2, shift_l3, - shift_l5, shift_l6, shift_l7}; - - return {base_lon, base_lat}; -} - -void shift_impl( - const std::vector & bases, std::vector * shift_lengths, - const ShiftInterval & shift_interval, const ShiftParameters & shift_parameters) -{ - // lateral shift - if (shift_interval.end <= 0.0) { - for (size_t i = 0; i < bases.size(); ++i) { - shift_lengths->at(i) += shift_interval.lateral_offset; - } - return; - } - - const double shift_arc_length = std::abs(shift_interval.end - shift_interval.start); - const bool shift_direction = shift_interval.end > shift_interval.start; - // Calculate base lengths - auto [base_lon, base_lat] = calc_base_lengths( - shift_arc_length, // - shift_interval.lateral_offset, // - shift_parameters); - - auto cubic_spline = - interpolator::CubicSpline::Builder{}.set_bases(base_lon).set_values(base_lat).build(); - - if (!cubic_spline) { - throw std::runtime_error( - "Failed to build cubic spline for shift calculation."); // This Exception should not be - // thrown. - } - for (size_t i = 0; i < bases.size(); ++i) { - // Calculate the shift length at the current base - const double s = bases.at(i); - if (shift_direction) { - if (s < shift_interval.start) { - continue; - } - if (s <= shift_interval.end) { - shift_lengths->at(i) += cubic_spline->compute(s - shift_interval.start); - } else { - shift_lengths->at(i) += cubic_spline->compute(shift_arc_length); - } - } else { - if (s > shift_interval.start) { - continue; - } - if (s >= shift_interval.end) { - shift_lengths->at(i) += cubic_spline->compute(shift_interval.start - s); - } else { - shift_lengths->at(i) += cubic_spline->compute(shift_arc_length); - } - } - } -} - -} // namespace autoware::trajectory::detail::impl diff --git a/common/autoware_trajectory/test/test_helpers.cpp b/common/autoware_trajectory/test/test_helpers.cpp deleted file mode 100644 index c5ab9ae6b4758..0000000000000 --- a/common/autoware_trajectory/test/test_helpers.cpp +++ /dev/null @@ -1,55 +0,0 @@ -// 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/trajectory/detail/helpers.hpp" - -#include - -#include - -TEST(TestHelpers, fill_bases) -{ - using autoware::trajectory::detail::fill_bases; - - std::vector x = {0.0, 1.0, 2.0, 3.0}; - size_t min_points = 9; - std::vector expected = {0.0, 1.0 / 3, 2.0 / 3, 1.0, 4.0 / 3, 5.0 / 3, 2.0, 2.5, 3.0}; - - auto result = fill_bases(x, min_points); - - EXPECT_EQ(result.size(), min_points); - - for (size_t i = 0; i < min_points; ++i) { - EXPECT_NEAR(result[i], expected[i], 1e-6); - } -} - -TEST(TestHelpers, crop_bases) -{ - using autoware::trajectory::detail::crop_bases; - - std::vector x = {0.0, 1.0, 2.0, 3.0, 4.0}; - double start = 1.5; - double end = 3.5; - - std::vector expected = {1.5, 2.0, 3.0, 3.5}; - - auto result = crop_bases(x, start, end); - - EXPECT_EQ(result.size(), expected.size()); - - for (size_t i = 0; i < expected.size(); ++i) { - EXPECT_NEAR(result[i], expected[i], 1e-6); - } -} diff --git a/common/autoware_trajectory/test/test_interpolator.cpp b/common/autoware_trajectory/test/test_interpolator.cpp deleted file mode 100644 index d00d48f8076bf..0000000000000 --- a/common/autoware_trajectory/test/test_interpolator.cpp +++ /dev/null @@ -1,121 +0,0 @@ -// 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/trajectory/interpolator/akima_spline.hpp" -#include "autoware/trajectory/interpolator/cubic_spline.hpp" -#include "autoware/trajectory/interpolator/linear.hpp" -#include "autoware/trajectory/interpolator/nearest_neighbor.hpp" -#include "autoware/trajectory/interpolator/spherical_linear.hpp" -#include "autoware/trajectory/interpolator/stairstep.hpp" - -#include - -#include - -#include -#include -#include - -template -class TestInterpolator : public ::testing::Test -{ -public: - std::optional interpolator; - std::vector bases; - std::vector values; - - void SetUp() override - { - // generate random values -1 to 1 - std::random_device rd; - std::mt19937 gen(rd()); - std::uniform_real_distribution<> dis(-1, 1); - bases.resize(10); - values.resize(10); - for (size_t i = 0; i < bases.size(); ++i) { - bases[i] = static_cast(i); - values[i] = dis(gen); - } - } -}; - -using Interpolators = testing::Types< - autoware::trajectory::interpolator::CubicSpline, autoware::trajectory::interpolator::AkimaSpline, - autoware::trajectory::interpolator::Linear, - autoware::trajectory::interpolator::NearestNeighbor, - autoware::trajectory::interpolator::Stairstep>; - -TYPED_TEST_SUITE(TestInterpolator, Interpolators, ); - -TYPED_TEST(TestInterpolator, compute) -{ - this->interpolator = - typename TypeParam::Builder().set_bases(this->bases).set_values(this->values).build(); - for (size_t i = 0; i < this->bases.size(); ++i) { - EXPECT_NEAR(this->values[i], this->interpolator->compute(this->bases[i]), 1e-6); - } -} - -// Instantiate test cases for all interpolators -template class TestInterpolator; -template class TestInterpolator; -template class TestInterpolator; -template class TestInterpolator>; -template class TestInterpolator>; - -/* - * Test SphericalLinear interpolator - */ - -geometry_msgs::msg::Quaternion create_quaternion(double w, double x, double y, double z) -{ - geometry_msgs::msg::Quaternion q; - q.w = w; - q.x = x; - q.y = y; - q.z = z; - return q; -} - -TEST(TestSphericalLinearInterpolator, compute) -{ - using autoware::trajectory::interpolator::SphericalLinear; - - std::vector bases = {0.0, 1.0}; - std::vector quaternions = { - create_quaternion(1.0, 0.0, 0.0, 0.0), create_quaternion(0.0, 1.0, 0.0, 0.0)}; - - auto interpolator = autoware::trajectory::interpolator::SphericalLinear::Builder() - .set_bases(bases) - .set_values(quaternions) - .build(); - - if (!interpolator) { - FAIL(); - } - - double s = 0.5; - geometry_msgs::msg::Quaternion result = interpolator->compute(s); - - // Expected values (from SLERP calculation) - double expected_w = std::sqrt(2.0) / 2.0; - double expected_x = std::sqrt(2.0) / 2.0; - double expected_y = 0.0; - double expected_z = 0.0; - - EXPECT_NEAR(result.w, expected_w, 1e-6); - EXPECT_NEAR(result.x, expected_x, 1e-6); - EXPECT_NEAR(result.y, expected_y, 1e-6); - EXPECT_NEAR(result.z, expected_z, 1e-6); -} diff --git a/common/autoware_trajectory/test/test_trajectory_container.cpp b/common/autoware_trajectory/test/test_trajectory_container.cpp deleted file mode 100644 index bdbf5eb60f524..0000000000000 --- a/common/autoware_trajectory/test/test_trajectory_container.cpp +++ /dev/null @@ -1,189 +0,0 @@ -// 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/trajectory/path_point_with_lane_id.hpp" -#include "autoware/trajectory/utils/closest.hpp" -#include "autoware/trajectory/utils/crossed.hpp" -#include "autoware/trajectory/utils/find_intervals.hpp" -#include "lanelet2_core/primitives/LineString.h" - -#include - -#include - -using Trajectory = - autoware::trajectory::Trajectory; - -autoware_internal_planning_msgs::msg::PathPointWithLaneId path_point_with_lane_id( - double x, double y, uint8_t lane_id) -{ - autoware_internal_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 TrajectoryTest : 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(TrajectoryTest, compute) -{ - double length = trajectory->length(); - - trajectory->longitudinal_velocity_mps() - .range(trajectory->length() / 3.0, trajectory->length()) - .set(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(TrajectoryTest, manipulate_velocity) -{ - trajectory->longitudinal_velocity_mps() = 10.0; - trajectory->longitudinal_velocity_mps() - .range(trajectory->length() / 3, 2.0 * trajectory->length() / 3) - .set(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(TrajectoryTest, direction) -{ - double dir = trajectory->azimuth(0.0); - EXPECT_LT(0, dir); - EXPECT_LT(dir, M_PI / 2); -} - -TEST_F(TrajectoryTest, curvature) -{ - double curvature_val = trajectory->curvature(0.0); - EXPECT_LT(-1.0, curvature_val); - EXPECT_LT(curvature_val, 1.0); -} - -TEST_F(TrajectoryTest, restore) -{ - using autoware::trajectory::Trajectory; - trajectory->longitudinal_velocity_mps().range(4.0, trajectory->length()).set(5.0); - auto points = trajectory->restore(0); - EXPECT_EQ(11, points.size()); -} - -TEST_F(TrajectoryTest, crossed) -{ - lanelet::LineString2d line_string; - line_string.push_back(lanelet::Point3d(lanelet::InvalId, 0.0, 10.0, 0.0)); - line_string.push_back(lanelet::Point3d(lanelet::InvalId, 10.0, 0.0, 0.0)); - - auto crossed_point = autoware::trajectory::crossed(*trajectory, line_string); - ASSERT_EQ(crossed_point.size(), 1); - - EXPECT_LT(0.0, crossed_point.at(0)); - EXPECT_LT(crossed_point.at(0), trajectory->length()); -} - -TEST_F(TrajectoryTest, closest) -{ - geometry_msgs::msg::Pose pose; - pose.position.x = 5.0; - pose.position.y = 5.0; - - auto closest_pose = trajectory->compute(autoware::trajectory::closest(*trajectory, 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(TrajectoryTest, 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]); -} - -TEST_F(TrajectoryTest, find_interval) -{ - auto intervals = autoware::trajectory::find_intervals( - *trajectory, [](const autoware_internal_planning_msgs::msg::PathPointWithLaneId & point) { - return point.lane_ids[0] == 1; - }); - EXPECT_EQ(intervals.size(), 1); - EXPECT_LT(0, intervals[0].start); - EXPECT_LT(intervals[0].start, intervals[0].end); - EXPECT_NEAR(intervals[0].end, trajectory->length(), 0.1); -}