From ca115d68b937e81e9433a8b466861b8b31cd9b28 Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Thu, 7 Nov 2024 20:11:44 +0900 Subject: [PATCH] feat(autoware_trajectory): move trajectory_container from autoware_motion_utils to a new package (#9253) * create trajectory container package Signed-off-by: Y.Hisaki * update Signed-off-by: Y.Hisaki * update Signed-off-by: Y.Hisaki * style(pre-commit): autofix * update codeowner Signed-off-by: Y.Hisaki * update Signed-off-by: Y.Hisaki * fix cmake Signed-off-by: Y.Hisaki --------- Signed-off-by: Y.Hisaki Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .github/CODEOWNERS | 1 + common/autoware_motion_utils/CMakeLists.txt | 26 -------- .../trajectory_container/interpolator.hpp | 23 ------- .../trajectory_container/trajectory.hpp | 21 ------ common/autoware_motion_utils/package.xml | 1 - common/autoware_trajectory/CMakeLists.txt | 50 +++++++++++++++ common/autoware_trajectory/README.md | 64 +++++++++++++++++++ .../examples/example_interpolator.cpp | 24 +++---- .../example_trajectory_path_point.cpp | 21 +++--- .../examples/example_trajectory_point.cpp | 10 +-- .../examples/example_trajectory_pose.cpp | 8 +-- .../trajectory/detail/interpolated_array.hpp | 12 ++-- .../autoware}/trajectory/detail/utils.hpp | 16 ++--- .../trajectory}/interpolator/akima_spline.hpp | 14 ++-- .../trajectory}/interpolator/cubic_spline.hpp | 14 ++-- .../detail/interpolator_common_interface.hpp | 12 ++-- .../detail/interpolator_mixin.hpp | 14 ++-- .../detail/nearest_neighbor_common_impl.hpp | 14 ++-- .../detail/stairstep_common_impl.hpp | 14 ++-- .../trajectory}/interpolator/interpolator.hpp | 14 ++-- .../trajectory}/interpolator/linear.hpp | 14 ++-- .../interpolator/nearest_neighbor.hpp | 14 ++-- .../interpolator/spherical_linear.hpp | 14 ++-- .../trajectory}/interpolator/stairstep.hpp | 14 ++-- .../autoware/trajectory/path_point.hpp} | 33 +++++----- .../trajectory/path_point_with_lane_id.hpp} | 28 ++++---- .../include/autoware/trajectory/point.hpp} | 34 +++++----- .../include/autoware/trajectory/pose.hpp} | 31 +++++---- common/autoware_trajectory/package.xml | 31 +++++++++ .../src}/detail/util.cpp | 36 +++++------ .../src}/interpolator/akima_spline.cpp | 8 +-- .../src}/interpolator/cubic_spline.cpp | 8 +-- .../src}/interpolator/linear.cpp | 8 +-- .../src}/interpolator/spherical_linear.cpp | 8 +-- .../src/path_point.cpp} | 20 +++--- .../src/path_point_with_lane_id.cpp} | 14 ++-- .../src/point.cpp} | 32 +++++----- .../src/pose.cpp} | 16 ++--- .../test}/test_interpolator.cpp | 43 ++++++------- .../test}/test_trajectory_container.cpp | 34 +++++----- 40 files changed, 439 insertions(+), 374 deletions(-) delete mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp delete mode 100644 common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory.hpp create mode 100644 common/autoware_trajectory/CMakeLists.txt create mode 100644 common/autoware_trajectory/README.md rename common/{autoware_motion_utils => autoware_trajectory}/examples/example_interpolator.cpp (75%) rename common/{autoware_motion_utils => autoware_trajectory}/examples/example_trajectory_path_point.cpp (75%) rename common/{autoware_motion_utils => autoware_trajectory}/examples/example_trajectory_point.cpp (89%) rename common/{autoware_motion_utils => autoware_trajectory}/examples/example_trajectory_pose.cpp (88%) rename common/{autoware_motion_utils/include/autoware/motion_utils/trajectory_container => autoware_trajectory/include/autoware}/trajectory/detail/interpolated_array.hpp (90%) rename common/{autoware_motion_utils/include/autoware/motion_utils/trajectory_container => autoware_trajectory/include/autoware}/trajectory/detail/utils.hpp (79%) rename common/{autoware_motion_utils/include/autoware/motion_utils/trajectory_container => autoware_trajectory/include/autoware/trajectory}/interpolator/akima_spline.hpp (82%) rename common/{autoware_motion_utils/include/autoware/motion_utils/trajectory_container => autoware_trajectory/include/autoware/trajectory}/interpolator/cubic_spline.hpp (83%) rename common/{autoware_motion_utils/include/autoware/motion_utils/trajectory_container => autoware_trajectory/include/autoware/trajectory}/interpolator/detail/interpolator_common_interface.hpp (88%) rename common/{autoware_motion_utils/include/autoware/motion_utils/trajectory_container => autoware_trajectory/include/autoware/trajectory}/interpolator/detail/interpolator_mixin.hpp (78%) rename common/{autoware_motion_utils/include/autoware/motion_utils/trajectory_container => autoware_trajectory/include/autoware/trajectory}/interpolator/detail/nearest_neighbor_common_impl.hpp (77%) rename common/{autoware_motion_utils/include/autoware/motion_utils/trajectory_container => autoware_trajectory/include/autoware/trajectory}/interpolator/detail/stairstep_common_impl.hpp (76%) rename common/{autoware_motion_utils/include/autoware/motion_utils/trajectory_container => autoware_trajectory/include/autoware/trajectory}/interpolator/interpolator.hpp (83%) rename common/{autoware_motion_utils/include/autoware/motion_utils/trajectory_container => autoware_trajectory/include/autoware/trajectory}/interpolator/linear.hpp (81%) rename common/{autoware_motion_utils/include/autoware/motion_utils/trajectory_container => autoware_trajectory/include/autoware/trajectory}/interpolator/nearest_neighbor.hpp (78%) rename common/{autoware_motion_utils/include/autoware/motion_utils/trajectory_container => autoware_trajectory/include/autoware/trajectory}/interpolator/spherical_linear.hpp (77%) rename common/{autoware_motion_utils/include/autoware/motion_utils/trajectory_container => autoware_trajectory/include/autoware/trajectory}/interpolator/stairstep.hpp (78%) rename common/{autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point.hpp => autoware_trajectory/include/autoware/trajectory/path_point.hpp} (76%) rename common/{autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point_with_lane_id.hpp => autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp} (78%) rename common/{autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_point.hpp => autoware_trajectory/include/autoware/trajectory/point.hpp} (87%) rename common/{autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_pose.hpp => autoware_trajectory/include/autoware/trajectory/pose.hpp} (69%) create mode 100644 common/autoware_trajectory/package.xml rename common/{autoware_motion_utils/src/trajectory_container/trajectory => autoware_trajectory/src}/detail/util.cpp (82%) rename common/{autoware_motion_utils/src/trajectory_container => autoware_trajectory/src}/interpolator/akima_spline.cpp (91%) rename common/{autoware_motion_utils/src/trajectory_container => autoware_trajectory/src}/interpolator/cubic_spline.cpp (91%) rename common/{autoware_motion_utils/src/trajectory_container => autoware_trajectory/src}/interpolator/linear.cpp (86%) rename common/{autoware_motion_utils/src/trajectory_container => autoware_trajectory/src}/interpolator/spherical_linear.cpp (87%) rename common/{autoware_motion_utils/src/trajectory_container/trajectory/trajectory_path_point.cpp => autoware_trajectory/src/path_point.cpp} (77%) rename common/{autoware_motion_utils/src/trajectory_container/trajectory/trajectory_path_point_with_lane_id.cpp => autoware_trajectory/src/path_point_with_lane_id.cpp} (76%) rename common/{autoware_motion_utils/src/trajectory_container/trajectory/trajectory_point.cpp => autoware_trajectory/src/point.cpp} (71%) rename common/{autoware_motion_utils/src/trajectory_container/trajectory/trajectory_pose.cpp => autoware_trajectory/src/pose.cpp} (83%) rename common/{autoware_motion_utils/test/src/trajectory_container => autoware_trajectory/test}/test_interpolator.cpp (65%) rename common/{autoware_motion_utils/test/src/trajectory_container => autoware_trajectory/test}/test_trajectory_container.cpp (83%) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 2fced720886b2..c9182ecde365d 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -22,6 +22,7 @@ common/autoware_signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.j common/autoware_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +common/autoware_trajectory_container/** yukinari.hisaki.2@tier4.jp takayuki.murooka@tier4.jp mamoru.sobue@tier4.jp common/autoware_universe_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp common/autoware_vehicle_info_utils/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp diff --git a/common/autoware_motion_utils/CMakeLists.txt b/common/autoware_motion_utils/CMakeLists.txt index 8cae2a8ac4110..4c36ef2f4e70d 100644 --- a/common/autoware_motion_utils/CMakeLists.txt +++ b/common/autoware_motion_utils/CMakeLists.txt @@ -1,8 +1,6 @@ cmake_minimum_required(VERSION 3.14) project(autoware_motion_utils) -option(BUILD_EXAMPLES "Build examples" OFF) - find_package(autoware_cmake REQUIRED) autoware_package() @@ -24,28 +22,4 @@ if(BUILD_TESTING) ) 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_motion_utils - matplotlibcpp17::matplotlibcpp17 - ) - endforeach() -endif() - ament_auto_package() diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp deleted file mode 100644 index beb3b9c7ead3e..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp +++ /dev/null @@ -1,23 +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__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_ -#include -#include -#include -#include -#include -#include -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory.hpp deleted file mode 100644 index 2e56dcf5b5521..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory.hpp +++ /dev/null @@ -1,21 +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__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY_HPP_ -#include -#include -#include -#include -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY_HPP_ diff --git a/common/autoware_motion_utils/package.xml b/common/autoware_motion_utils/package.xml index c3429022e3a59..1fe2c4d8e1ea8 100644 --- a/common/autoware_motion_utils/package.xml +++ b/common/autoware_motion_utils/package.xml @@ -34,7 +34,6 @@ tf2_geometry_msgs tier4_planning_msgs visualization_msgs - ament_cmake_ros ament_lint_auto diff --git a/common/autoware_trajectory/CMakeLists.txt b/common/autoware_trajectory/CMakeLists.txt new file mode 100644 index 0000000000000..9ce9ddf79bf14 --- /dev/null +++ b/common/autoware_trajectory/CMakeLists.txt @@ -0,0 +1,50 @@ +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 new file mode 100644 index 0000000000000..bddeef3b96eae --- /dev/null +++ b/common/autoware_trajectory/README.md @@ -0,0 +1,64 @@ +# 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_motion_utils/examples/example_interpolator.cpp b/common/autoware_trajectory/examples/example_interpolator.cpp similarity index 75% rename from common/autoware_motion_utils/examples/example_interpolator.cpp rename to common/autoware_trajectory/examples/example_interpolator.cpp index 6c17975c74359..f34d291377884 100644 --- a/common/autoware_motion_utils/examples/example_interpolator.cpp +++ b/common/autoware_trajectory/examples/example_interpolator.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp" -#include "autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp" -#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" -#include "autoware/motion_utils/trajectory_container/interpolator/linear.hpp" -#include "autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp" -#include "autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp" +#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 @@ -41,10 +41,10 @@ int main() // Scatter Data plt.scatter(Args(bases, values)); - using autoware::motion_utils::trajectory_container::interpolator::InterpolatorInterface; + using autoware::trajectory::interpolator::InterpolatorInterface; // Linear Interpolator { - using autoware::motion_utils::trajectory_container::interpolator::Linear; + using autoware::trajectory::interpolator::Linear; auto interpolator = *Linear::Builder{}.set_bases(bases).set_values(values).build(); std::vector x; std::vector y; @@ -57,7 +57,7 @@ int main() // AkimaSpline Interpolator { - using autoware::motion_utils::trajectory_container::interpolator::AkimaSpline; + using autoware::trajectory::interpolator::AkimaSpline; auto interpolator = *AkimaSpline::Builder{}.set_bases(bases).set_values(values).build(); std::vector x; @@ -71,7 +71,7 @@ int main() // CubicSpline Interpolator { - using autoware::motion_utils::trajectory_container::interpolator::CubicSpline; + using autoware::trajectory::interpolator::CubicSpline; auto interpolator = *CubicSpline::Builder{}.set_bases(bases).set_values(values).build(); std::vector x; std::vector y; @@ -84,7 +84,7 @@ int main() // NearestNeighbor Interpolator { - using autoware::motion_utils::trajectory_container::interpolator::NearestNeighbor; + using autoware::trajectory::interpolator::NearestNeighbor; auto interpolator = *NearestNeighbor::Builder{}.set_bases(bases).set_values(values).build(); std::vector x; @@ -98,7 +98,7 @@ int main() // Stairstep Interpolator { - using autoware::motion_utils::trajectory_container::interpolator::Stairstep; + using autoware::trajectory::interpolator::Stairstep; auto interpolator = *Stairstep::Builder{}.set_bases(bases).set_values(values).build(); std::vector x; std::vector y; diff --git a/common/autoware_motion_utils/examples/example_trajectory_path_point.cpp b/common/autoware_trajectory/examples/example_trajectory_path_point.cpp similarity index 75% rename from common/autoware_motion_utils/examples/example_trajectory_path_point.cpp rename to common/autoware_trajectory/examples/example_trajectory_path_point.cpp index 2f5aedc9c11e1..00f204dd24e35 100644 --- a/common/autoware_motion_utils/examples/example_trajectory_path_point.cpp +++ b/common/autoware_trajectory/examples/example_trajectory_path_point.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point.hpp" +#include "autoware/trajectory/path_point.hpp" #include #include @@ -21,7 +21,7 @@ #include -autoware_planning_msgs::msg::PathPoint pose(double x, double y) +autoware_planning_msgs::msg::PathPoint path_point(double x, double y) { autoware_planning_msgs::msg::PathPoint p; p.pose.position.x = x; @@ -32,18 +32,20 @@ autoware_planning_msgs::msg::PathPoint pose(double x, double y) int main() { - using autoware::motion_utils::trajectory_container::trajectory::TrajectoryContainer; + using autoware::trajectory::Trajectory; 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)}; + 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; @@ -57,8 +59,7 @@ int main() plt.scatter(Args(x, y), Kwargs("label"_a = "Original", "color"_a = "red")); } - auto trajectory = - TrajectoryContainer::Builder{}.build(points); + auto trajectory = Trajectory::Builder{}.build(points); if (!trajectory) { std::cerr << "Failed to build trajectory" << std::endl; diff --git a/common/autoware_motion_utils/examples/example_trajectory_point.cpp b/common/autoware_trajectory/examples/example_trajectory_point.cpp similarity index 89% rename from common/autoware_motion_utils/examples/example_trajectory_point.cpp rename to common/autoware_trajectory/examples/example_trajectory_point.cpp index 42c915896e460..014f27ffc9a3a 100644 --- a/common/autoware_motion_utils/examples/example_trajectory_point.cpp +++ b/common/autoware_trajectory/examples/example_trajectory_point.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp" -#include "autoware/motion_utils/trajectory_container/trajectory/trajectory_point.hpp" +#include "autoware/trajectory/interpolator/cubic_spline.hpp" +#include "autoware/trajectory/point.hpp" #include @@ -55,10 +55,10 @@ int main() plt.scatter(Args(x, y), Kwargs("label"_a = "Original", "color"_a = "red")); } - using autoware::motion_utils::trajectory_container::interpolator::CubicSpline; - using autoware::motion_utils::trajectory_container::trajectory::TrajectoryContainer; + using autoware::trajectory::Trajectory; + using autoware::trajectory::interpolator::CubicSpline; - auto trajectory = TrajectoryContainer::Builder() + auto trajectory = Trajectory::Builder() .set_xy_interpolator() .set_z_interpolator() .build(points); diff --git a/common/autoware_motion_utils/examples/example_trajectory_pose.cpp b/common/autoware_trajectory/examples/example_trajectory_pose.cpp similarity index 88% rename from common/autoware_motion_utils/examples/example_trajectory_pose.cpp rename to common/autoware_trajectory/examples/example_trajectory_pose.cpp index 678176c39e99c..a6ca1310fdc42 100644 --- a/common/autoware_motion_utils/examples/example_trajectory_pose.cpp +++ b/common/autoware_trajectory/examples/example_trajectory_pose.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/motion_utils/trajectory_container/trajectory/trajectory_pose.hpp" +#include "autoware/trajectory/pose.hpp" #include @@ -44,10 +44,10 @@ int main() pose(6.88, 3.54), pose(7.51, 4.25), pose(7.85, 4.93), pose(8.03, 5.73), pose(8.16, 6.52), pose(8.31, 7.28), pose(8.45, 7.93), pose(8.68, 8.45), pose(8.96, 8.96), pose(9.32, 9.36)}; - using autoware::motion_utils::trajectory_container::interpolator::CubicSpline; - using autoware::motion_utils::trajectory_container::trajectory::TrajectoryContainer; + using autoware::trajectory::Trajectory; + using autoware::trajectory::interpolator::CubicSpline; - auto trajectory = TrajectoryContainer::Builder{}.build(poses); + auto trajectory = Trajectory::Builder{}.build(poses); trajectory->align_orientation_with_trajectory_direction(); diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/detail/interpolated_array.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp similarity index 90% rename from common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/detail/interpolated_array.hpp rename to common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp index e00d1aba61e34..d4ea4b5a8dee4 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/detail/interpolated_array.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/detail/interpolated_array.hpp @@ -13,11 +13,11 @@ // limitations under the License. // clang-format off -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ // NOLINT -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ // NOLINT +#ifndef AUTOWARE__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ // NOLINT +#define AUTOWARE__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ // NOLINT // clang-format on -#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" +#include "autoware/trajectory/interpolator/interpolator.hpp" #include #include @@ -27,7 +27,7 @@ #include #include -namespace autoware::motion_utils::trajectory_container::trajectory::detail +namespace autoware::trajectory::detail { /** @@ -203,8 +203,8 @@ class InterpolatedArray std::pair, std::vector> get_data() const { return {bases_, values_}; } }; -} // namespace autoware::motion_utils::trajectory_container::trajectory::detail +} // namespace autoware::trajectory::detail // clang-format off -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ // NOLINT +#endif // AUTOWARE__TRAJECTORY__DETAIL__INTERPOLATED_ARRAY_HPP_ // NOLINT // clang-format on diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/detail/utils.hpp b/common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp similarity index 79% rename from common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/detail/utils.hpp rename to common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp index 9aca9ba549c2b..9a33152a7dfdb 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/detail/utils.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/detail/utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__DETAIL__UTILS_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__DETAIL__UTILS_HPP_ +#ifndef AUTOWARE__TRAJECTORY__DETAIL__UTILS_HPP_ +#define AUTOWARE__TRAJECTORY__DETAIL__UTILS_HPP_ -// #include "lanelet2_core/primitives/Point.h" +#include "lanelet2_core/primitives/Point.h" #include @@ -27,7 +27,7 @@ #include #include -namespace autoware::motion_utils::trajectory_container::trajectory::detail +namespace autoware::trajectory::detail { /** * @brief Convert various point types to geometry_msgs::msg::Point. @@ -39,8 +39,8 @@ geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Pose & p); geometry_msgs::msg::Point to_point(const Eigen::Ref & p); geometry_msgs::msg::Point to_point(const autoware_planning_msgs::msg::PathPoint & p); geometry_msgs::msg::Point to_point(const tier4_planning_msgs::msg::PathPointWithLaneId & p); -// geometry_msgs::msg::Point to_point(const lanelet::BasicPoint2d & p); -// geometry_msgs::msg::Point to_point(const lanelet::ConstPoint3d & p); +geometry_msgs::msg::Point to_point(const lanelet::BasicPoint2d & p); +geometry_msgs::msg::Point to_point(const lanelet::ConstPoint3d & p); /** * @brief Merge multiple vectors into one, keeping only unique elements. @@ -70,6 +70,6 @@ std::vector fill_bases(const std::vector & x, const size_t & min std::vector crop_bases( const std::vector & x, const double & start, const double & end); -} // namespace autoware::motion_utils::trajectory_container::trajectory::detail +} // namespace autoware::trajectory::detail -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__DETAIL__UTILS_HPP_ +#endif // AUTOWARE__TRAJECTORY__DETAIL__UTILS_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/akima_spline.hpp similarity index 82% rename from common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp rename to common/autoware_trajectory/include/autoware/trajectory/interpolator/akima_spline.hpp index f9a2ac1316a4e..e3484b0205200 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/akima_spline.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_ +#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__AKIMA_SPLINE_HPP_ +#define AUTOWARE__TRAJECTORY__INTERPOLATOR__AKIMA_SPLINE_HPP_ -#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp" +#include "autoware/trajectory/interpolator/detail/interpolator_mixin.hpp" #include #include -namespace autoware::motion_utils::trajectory_container::interpolator +namespace autoware::trajectory::interpolator { /** @@ -89,6 +89,6 @@ class AkimaSpline : public detail::InterpolatorMixin [[nodiscard]] size_t minimum_required_points() const override { return 5; } }; -} // namespace autoware::motion_utils::trajectory_container::interpolator +} // namespace autoware::trajectory::interpolator -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__AKIMA_SPLINE_HPP_ +#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__AKIMA_SPLINE_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/cubic_spline.hpp similarity index 83% rename from common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp rename to common/autoware_trajectory/include/autoware/trajectory/interpolator/cubic_spline.hpp index 437ddd727cc7d..51694b15f8932 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/cubic_spline.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_ +#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__CUBIC_SPLINE_HPP_ +#define AUTOWARE__TRAJECTORY__INTERPOLATOR__CUBIC_SPLINE_HPP_ -#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp" +#include "autoware/trajectory/interpolator/detail/interpolator_mixin.hpp" #include #include -namespace autoware::motion_utils::trajectory_container::interpolator +namespace autoware::trajectory::interpolator { /** @@ -91,6 +91,6 @@ class CubicSpline : public detail::InterpolatorMixin [[nodiscard]] size_t minimum_required_points() const override { return 4; } }; -} // namespace autoware::motion_utils::trajectory_container::interpolator +} // namespace autoware::trajectory::interpolator -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__CUBIC_SPLINE_HPP_ +#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__CUBIC_SPLINE_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_interface.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_common_interface.hpp similarity index 88% rename from common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_interface.hpp rename to common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_common_interface.hpp index f0131e85e157c..9ce92e9bd02ea 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_interface.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_common_interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,8 +13,8 @@ // limitations under the License. // clang-format off -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_ // NOLINT -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_ // NOLINT +#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_ // NOLINT +#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_ // NOLINT // clang-format on #include @@ -22,7 +22,7 @@ #include -namespace autoware::motion_utils::trajectory_container::interpolator::detail +namespace autoware::trajectory::interpolator::detail { /** * @brief Base class for interpolation implementations. @@ -136,8 +136,8 @@ class InterpolatorCommonInterface return compute_impl(s); } }; -} // namespace autoware::motion_utils::trajectory_container::interpolator::detail +} // namespace autoware::trajectory::interpolator::detail // clang-format off -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_ // NOLINT +#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_COMMON_INTERFACE_HPP_ // NOLINT // clang-format on diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_mixin.hpp similarity index 78% rename from common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp rename to common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_mixin.hpp index c5446e945a799..a71bdcf0067cf 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/interpolator_mixin.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,11 +13,11 @@ // limitations under the License. // clang-format off -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_ // NOLINT -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_ // NOLINT +#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_ // NOLINT +#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_ // NOLINT // clang-format on -#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" +#include "autoware/trajectory/interpolator/interpolator.hpp" #include @@ -26,7 +26,7 @@ #include #include -namespace autoware::motion_utils::trajectory_container::interpolator::detail +namespace autoware::trajectory::interpolator::detail { /** @@ -83,8 +83,8 @@ struct InterpolatorMixin : public InterpolatorInterface }; }; -} // namespace autoware::motion_utils::trajectory_container::interpolator::detail +} // namespace autoware::trajectory::interpolator::detail // clang-format off -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_ // NOLINT +#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__INTERPOLATOR_MIXIN_HPP_ // NOLINT // clang-format on diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/nearest_neighbor_common_impl.hpp similarity index 77% rename from common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp rename to common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/nearest_neighbor_common_impl.hpp index ad0e2b98ef040..94df905ef4815 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/detail/nearest_neighbor_common_impl.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,15 +13,15 @@ // limitations under the License. // clang-format off -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_ // NOLINT -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_ // NOLINT +#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_ // NOLINT +#define AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__NEAREST_NEIGHBOR_COMMON_IMPL_HPP_ // NOLINT // clang-format on -#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp" +#include "autoware/trajectory/interpolator/detail/interpolator_mixin.hpp" #include -namespace autoware::motion_utils::trajectory_container::interpolator +namespace autoware::trajectory::interpolator { template @@ -79,7 +79,7 @@ class NearestNeighborCommonImpl : public detail::InterpolatorMixin -namespace autoware::motion_utils::trajectory_container::interpolator +namespace autoware::trajectory::interpolator { template @@ -78,8 +78,8 @@ class StairstepCommonImpl : public detail::InterpolatorMixin, T> [[nodiscard]] size_t minimum_required_points() const override { return 2; } }; } // namespace detail -} // namespace autoware::motion_utils::trajectory_container::interpolator +} // namespace autoware::trajectory::interpolator // clang-format off -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_ // NOLINT +#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__DETAIL__STAIRSTEP_COMMON_IMPL_HPP_ // NOLINT // clang-format on diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp similarity index 83% rename from common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp rename to common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp index 03827ded59cb6..8503d658a1202 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/interpolator.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_HPP_ +#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__INTERPOLATOR_HPP_ +#define AUTOWARE__TRAJECTORY__INTERPOLATOR__INTERPOLATOR_HPP_ -#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_common_interface.hpp" +#include "autoware/trajectory/interpolator/detail/interpolator_common_interface.hpp" #include -namespace autoware::motion_utils::trajectory_container::interpolator +namespace autoware::trajectory::interpolator { /** * @brief Template class for interpolation. @@ -92,6 +92,6 @@ class InterpolatorInterface : public detail::InterpolatorCommonInterface [[nodiscard]] virtual std::shared_ptr> clone() const = 0; }; -} // namespace autoware::motion_utils::trajectory_container::interpolator +} // namespace autoware::trajectory::interpolator -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__INTERPOLATOR_HPP_ +#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__INTERPOLATOR_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/linear.hpp similarity index 81% rename from common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp rename to common/autoware_trajectory/include/autoware/trajectory/interpolator/linear.hpp index 99fcb469365b8..bef2cde70fff7 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/linear.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/linear.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__LINEAR_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__LINEAR_HPP_ +#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__LINEAR_HPP_ +#define AUTOWARE__TRAJECTORY__INTERPOLATOR__LINEAR_HPP_ -#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp" +#include "autoware/trajectory/interpolator/detail/interpolator_mixin.hpp" #include #include -namespace autoware::motion_utils::trajectory_container::interpolator +namespace autoware::trajectory::interpolator { /** @@ -81,6 +81,6 @@ class Linear : public detail::InterpolatorMixin [[nodiscard]] size_t minimum_required_points() const override; }; -} // namespace autoware::motion_utils::trajectory_container::interpolator +} // namespace autoware::trajectory::interpolator -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__LINEAR_HPP_ +#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__LINEAR_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/nearest_neighbor.hpp similarity index 78% rename from common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp rename to common/autoware_trajectory/include/autoware/trajectory/interpolator/nearest_neighbor.hpp index 72b61001b05e3..959c8cfb3a5fb 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/nearest_neighbor.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/nearest_neighbor.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__NEAREST_NEIGHBOR_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__NEAREST_NEIGHBOR_HPP_ +#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__NEAREST_NEIGHBOR_HPP_ +#define AUTOWARE__TRAJECTORY__INTERPOLATOR__NEAREST_NEIGHBOR_HPP_ -#include "autoware/motion_utils/trajectory_container/interpolator/detail/nearest_neighbor_common_impl.hpp" +#include "autoware/trajectory/interpolator/detail/nearest_neighbor_common_impl.hpp" -namespace autoware::motion_utils::trajectory_container::interpolator +namespace autoware::trajectory::interpolator { /** @@ -73,6 +73,6 @@ class NearestNeighbor : public detail::NearestNeighborCommonImpl NearestNeighbor() = default; }; -} // namespace autoware::motion_utils::trajectory_container::interpolator +} // namespace autoware::trajectory::interpolator -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__NEAREST_NEIGHBOR_HPP_ +#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__NEAREST_NEIGHBOR_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/spherical_linear.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/spherical_linear.hpp similarity index 77% rename from common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/spherical_linear.hpp rename to common/autoware_trajectory/include/autoware/trajectory/interpolator/spherical_linear.hpp index 6e47fb856e5b9..c443daf114d0d 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/spherical_linear.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/spherical_linear.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__SPHERICAL_LINEAR_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__SPHERICAL_LINEAR_HPP_ +#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__SPHERICAL_LINEAR_HPP_ +#define AUTOWARE__TRAJECTORY__INTERPOLATOR__SPHERICAL_LINEAR_HPP_ -#include "autoware/motion_utils/trajectory_container/interpolator/detail/interpolator_mixin.hpp" +#include "autoware/trajectory/interpolator/detail/interpolator_mixin.hpp" #include #include -namespace autoware::motion_utils::trajectory_container::interpolator +namespace autoware::trajectory::interpolator { /** @@ -68,6 +68,6 @@ class SphericalLinear [[nodiscard]] size_t minimum_required_points() const override; }; -} // namespace autoware::motion_utils::trajectory_container::interpolator +} // namespace autoware::trajectory::interpolator -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__SPHERICAL_LINEAR_HPP_ +#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__SPHERICAL_LINEAR_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp b/common/autoware_trajectory/include/autoware/trajectory/interpolator/stairstep.hpp similarity index 78% rename from common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp rename to common/autoware_trajectory/include/autoware/trajectory/interpolator/stairstep.hpp index 801b207b25afc..ad7ac8c7e53af 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/interpolator/stairstep.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__STAIRSTEP_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__STAIRSTEP_HPP_ +#ifndef AUTOWARE__TRAJECTORY__INTERPOLATOR__STAIRSTEP_HPP_ +#define AUTOWARE__TRAJECTORY__INTERPOLATOR__STAIRSTEP_HPP_ -#include "autoware/motion_utils/trajectory_container/interpolator/detail/stairstep_common_impl.hpp" +#include "autoware/trajectory/interpolator/detail/stairstep_common_impl.hpp" -namespace autoware::motion_utils::trajectory_container::interpolator +namespace autoware::trajectory::interpolator { /** @@ -73,6 +73,6 @@ class Stairstep : public detail::StairstepCommonImpl Stairstep() = default; }; -} // namespace autoware::motion_utils::trajectory_container::interpolator +} // namespace autoware::trajectory::interpolator -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__INTERPOLATOR__STAIRSTEP_HPP_ +#endif // AUTOWARE__TRAJECTORY__INTERPOLATOR__STAIRSTEP_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point.hpp b/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp similarity index 76% rename from common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point.hpp rename to common/autoware_trajectory/include/autoware/trajectory/path_point.hpp index 3341e38cd87ae..c14a5b023240d 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/path_point.hpp @@ -12,27 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_PATH_POINT_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_PATH_POINT_HPP_ +#ifndef AUTOWARE__TRAJECTORY__PATH_POINT_HPP_ +#define AUTOWARE__TRAJECTORY__PATH_POINT_HPP_ -#include "autoware/motion_utils/trajectory_container/interpolator/stairstep.hpp" -#include "autoware/motion_utils/trajectory_container/trajectory/detail/interpolated_array.hpp" -#include "autoware/motion_utils/trajectory_container/trajectory/trajectory_pose.hpp" - -// #include -// #include +#include "autoware/trajectory/detail/interpolated_array.hpp" +#include "autoware/trajectory/interpolator/stairstep.hpp" +#include "autoware/trajectory/pose.hpp" #include #include #include -namespace autoware::motion_utils::trajectory_container::trajectory +namespace autoware::trajectory { template <> -class TrajectoryContainer -: public TrajectoryContainer +class Trajectory +: public Trajectory { - using BaseClass = TrajectoryContainer; + using BaseClass = Trajectory; using PointType = autoware_planning_msgs::msg::PathPoint; public: @@ -65,12 +62,12 @@ class TrajectoryContainer class Builder { private: - std::unique_ptr trajectory_; + std::unique_ptr trajectory_; public: Builder() { - trajectory_ = std::make_unique(); + trajectory_ = std::make_unique(); set_xy_interpolator(); set_z_interpolator(); set_orientation_interpolator(); @@ -129,10 +126,10 @@ class TrajectoryContainer return *this; } - std::optional build(const std::vector & points) + std::optional build(const std::vector & points) { if (trajectory_->build(points)) { - auto result = std::make_optional(std::move(*trajectory_)); + auto result = std::make_optional(std::move(*trajectory_)); trajectory_.reset(); return result; } @@ -141,6 +138,6 @@ class TrajectoryContainer }; }; -} // namespace autoware::motion_utils::trajectory_container::trajectory +} // namespace autoware::trajectory -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_PATH_POINT_HPP_ +#endif // AUTOWARE__TRAJECTORY__PATH_POINT_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point_with_lane_id.hpp b/common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp similarity index 78% rename from common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point_with_lane_id.hpp rename to common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp index dfad421da4881..776779396db41 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point_with_lane_id.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/path_point_with_lane_id.hpp @@ -13,12 +13,12 @@ // limitations under the License. // clang-format off -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_PATH_POINT_WITH_LANE_ID_HPP_ // NOLINT -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_PATH_POINT_WITH_LANE_ID_HPP_ // NOLINT +#ifndef AUTOWARE__TRAJECTORY__PATH_POINT_WITH_LANE_ID_HPP_ // NOLINT +#define AUTOWARE__TRAJECTORY__PATH_POINT_WITH_LANE_ID_HPP_ // NOLINT // clang-format on -#include "autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point.hpp" -#include "autoware/motion_utils/trajectory_container/trajectory/trajectory_point.hpp" +#include "autoware/trajectory/path_point.hpp" +#include "autoware/trajectory/point.hpp" #include @@ -26,13 +26,13 @@ #include #include -namespace autoware::motion_utils::trajectory_container::trajectory +namespace autoware::trajectory { template <> -class TrajectoryContainer -: public TrajectoryContainer +class Trajectory +: public Trajectory { - using BaseClass = TrajectoryContainer; + using BaseClass = Trajectory; using PointType = tier4_planning_msgs::msg::PathPointWithLaneId; using LaneIdType = std::vector; @@ -63,12 +63,12 @@ class TrajectoryContainer class Builder { private: - std::unique_ptr trajectory_; + std::unique_ptr trajectory_; public: Builder() { - trajectory_ = std::make_unique(); + trajectory_ = std::make_unique(); set_xy_interpolator(); set_z_interpolator(); set_orientation_interpolator(); @@ -136,10 +136,10 @@ class TrajectoryContainer return *this; } - std::optional build(const std::vector & points) + std::optional build(const std::vector & points) { if (trajectory_->build(points)) { - auto result = std::make_optional(std::move(*trajectory_)); + auto result = std::make_optional(std::move(*trajectory_)); trajectory_.reset(); return result; } @@ -147,8 +147,8 @@ class TrajectoryContainer } }; }; -} // namespace autoware::motion_utils::trajectory_container::trajectory +} // namespace autoware::trajectory // clang-format off -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_PATH_POINT_WITH_LANE_ID_HPP_ // NOLINT +#endif // AUTOWARE__TRAJECTORY__PATH_POINT_WITH_LANE_ID_HPP_ // NOLINT // clang-format on diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_point.hpp b/common/autoware_trajectory/include/autoware/trajectory/point.hpp similarity index 87% rename from common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_point.hpp rename to common/autoware_trajectory/include/autoware/trajectory/point.hpp index 81065924007dc..cdc5440e62eab 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_point.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/point.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_POINT_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_POINT_HPP_ +#ifndef AUTOWARE__TRAJECTORY__POINT_HPP_ +#define AUTOWARE__TRAJECTORY__POINT_HPP_ -#include "autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp" -#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" -#include "autoware/motion_utils/trajectory_container/interpolator/linear.hpp" -#include "autoware/motion_utils/trajectory_container/trajectory/detail/utils.hpp" +#include "autoware/trajectory/detail/utils.hpp" +#include "autoware/trajectory/interpolator/cubic_spline.hpp" +#include "autoware/trajectory/interpolator/interpolator.hpp" +#include "autoware/trajectory/interpolator/linear.hpp" #include @@ -32,17 +32,17 @@ #include #include -namespace autoware::motion_utils::trajectory_container::trajectory +namespace autoware::trajectory { template -class TrajectoryContainer; +class Trajectory; /** * @brief Trajectory class for geometry_msgs::msg::Point */ template <> -class TrajectoryContainer +class Trajectory { using PointType = geometry_msgs::msg::Point; @@ -119,7 +119,7 @@ class TrajectoryContainer [[nodiscard]] std::optional closest_with_constraint( const InputPointType & p, const ConstraintFunction & constraints) const { - using motion_utils::trajectory_container::trajectory::detail::to_point; + using trajectory::detail::to_point; Eigen::Vector2d point(to_point(p).x, to_point(p).y); std::vector distances_from_segments; std::vector lengthes_from_start_points; @@ -187,7 +187,7 @@ class TrajectoryContainer const InputPointType & start, const InputPointType & end, const ConstraintFunction & constraints) const { - using motion_utils::trajectory_container::trajectory::detail::to_point; + using trajectory::detail::to_point; Eigen::Vector2d line_start(to_point(start).x, to_point(start).y); Eigen::Vector2d line_end(to_point(end).x, to_point(end).y); Eigen::Vector2d line_dir = line_end - line_start; @@ -251,12 +251,12 @@ class TrajectoryContainer class Builder { private: - std::unique_ptr trajectory_; + std::unique_ptr trajectory_; public: Builder() { - trajectory_ = std::make_unique(); + trajectory_ = std::make_unique(); // Default interpolators set_xy_interpolator(); set_z_interpolator(); @@ -280,10 +280,10 @@ class TrajectoryContainer return *this; } - std::optional build(const std::vector & points) + std::optional build(const std::vector & points) { if (trajectory_->build(points)) { - auto result = std::make_optional(std::move(*trajectory_)); + auto result = std::make_optional(std::move(*trajectory_)); trajectory_.reset(); return result; } @@ -292,6 +292,6 @@ class TrajectoryContainer }; }; -} // namespace autoware::motion_utils::trajectory_container::trajectory +} // namespace autoware::trajectory -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_POINT_HPP_ +#endif // AUTOWARE__TRAJECTORY__POINT_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_pose.hpp b/common/autoware_trajectory/include/autoware/trajectory/pose.hpp similarity index 69% rename from common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_pose.hpp rename to common/autoware_trajectory/include/autoware/trajectory/pose.hpp index ffebaf290d939..90d84ef0a6a90 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory/trajectory_pose.hpp +++ b/common/autoware_trajectory/include/autoware/trajectory/pose.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_POSE_HPP_ -#define AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_POSE_HPP_ +#ifndef AUTOWARE__TRAJECTORY__POSE_HPP_ +#define AUTOWARE__TRAJECTORY__POSE_HPP_ -#include "autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp" -#include "autoware/motion_utils/trajectory_container/interpolator/interpolator.hpp" -#include "autoware/motion_utils/trajectory_container/interpolator/spherical_linear.hpp" -#include "autoware/motion_utils/trajectory_container/trajectory/trajectory_point.hpp" +#include "autoware/trajectory/interpolator/cubic_spline.hpp" +#include "autoware/trajectory/interpolator/interpolator.hpp" +#include "autoware/trajectory/interpolator/spherical_linear.hpp" +#include "autoware/trajectory/point.hpp" #include @@ -26,17 +26,16 @@ #include #include -namespace autoware::motion_utils::trajectory_container::trajectory +namespace autoware::trajectory { /** * @brief Trajectory class for geometry_msgs::msg::Pose */ template <> -class TrajectoryContainer -: public TrajectoryContainer +class Trajectory : public Trajectory { - using BaseClass = TrajectoryContainer; + using BaseClass = Trajectory; using PointType = geometry_msgs::msg::Pose; protected: @@ -67,12 +66,12 @@ class TrajectoryContainer class Builder { private: - std::unique_ptr trajectory_; + std::unique_ptr trajectory_; public: Builder() { - trajectory_ = std::make_unique(); + trajectory_ = std::make_unique(); set_xy_interpolator(); set_z_interpolator(); set_orientation_interpolator(); @@ -104,10 +103,10 @@ class TrajectoryContainer return *this; } - std::optional build(const std::vector & points) + std::optional build(const std::vector & points) { if (trajectory_->build(points)) { - auto result = std::make_optional(std::move(*trajectory_)); + auto result = std::make_optional(std::move(*trajectory_)); trajectory_.reset(); return result; } @@ -116,6 +115,6 @@ class TrajectoryContainer }; }; -} // namespace autoware::motion_utils::trajectory_container::trajectory +} // namespace autoware::trajectory -#endif // AUTOWARE__MOTION_UTILS__TRAJECTORY_CONTAINER__TRAJECTORY__TRAJECTORY_POSE_HPP_ +#endif // AUTOWARE__TRAJECTORY__POSE_HPP_ diff --git a/common/autoware_trajectory/package.xml b/common/autoware_trajectory/package.xml new file mode 100644 index 0000000000000..d6ab5465578aa --- /dev/null +++ b/common/autoware_trajectory/package.xml @@ -0,0 +1,31 @@ + + + + autoware_trajectory + 0.1.0 + The autoware_trajectory package + Yukinari Hisaki + Takayuki Murooka + Mamoru Sobue + Apache License 2.0 + + Yukinari Hisaki + + ament_cmake_auto + autoware_cmake + + autoware_planning_msgs + geometry_msgs + lanelet2_core + rclcpp + tf2 + tier4_planning_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/autoware_motion_utils/src/trajectory_container/trajectory/detail/util.cpp b/common/autoware_trajectory/src/detail/util.cpp similarity index 82% rename from common/autoware_motion_utils/src/trajectory_container/trajectory/detail/util.cpp rename to common/autoware_trajectory/src/detail/util.cpp index 9886036604109..8cfb8029629cb 100644 --- a/common/autoware_motion_utils/src/trajectory_container/trajectory/detail/util.cpp +++ b/common/autoware_trajectory/src/detail/util.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/motion_utils/trajectory_container/trajectory/detail/utils.hpp" +#include "autoware/trajectory/detail/utils.hpp" #include #include -namespace autoware::motion_utils::trajectory_container::trajectory::detail +namespace autoware::trajectory::detail { geometry_msgs::msg::Point to_point(const geometry_msgs::msg::Point & p) @@ -54,21 +54,21 @@ geometry_msgs::msg::Point to_point(const tier4_planning_msgs::msg::PathPointWith return point; } -// geometry_msgs::msg::Point to_point(const lanelet::BasicPoint2d & p) -// { -// geometry_msgs::msg::Point point; -// point.x = p.x(); -// point.y = p.y(); -// return point; -// } - -// geometry_msgs::msg::Point to_point(const lanelet::ConstPoint3d & p) -// { -// geometry_msgs::msg::Point point; -// point.x = p.x(); -// point.y = p.y(); -// return point; -// } +geometry_msgs::msg::Point to_point(const lanelet::BasicPoint2d & p) +{ + geometry_msgs::msg::Point point; + point.x = p.x(); + point.y = p.y(); + return point; +} + +geometry_msgs::msg::Point to_point(const lanelet::ConstPoint3d & p) +{ + geometry_msgs::msg::Point point; + point.x = p.x(); + point.y = p.y(); + return point; +} std::vector fill_bases(const std::vector & x, const size_t & min_points) { @@ -129,4 +129,4 @@ std::vector crop_bases( return result; } -} // namespace autoware::motion_utils::trajectory_container::trajectory::detail +} // namespace autoware::trajectory::detail diff --git a/common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp b/common/autoware_trajectory/src/interpolator/akima_spline.cpp similarity index 91% rename from common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp rename to common/autoware_trajectory/src/interpolator/akima_spline.cpp index 747362ac9167e..766d01ca85bbb 100644 --- a/common/autoware_motion_utils/src/trajectory_container/interpolator/akima_spline.cpp +++ b/common/autoware_trajectory/src/interpolator/akima_spline.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/motion_utils/trajectory_container/interpolator/akima_spline.hpp" +#include "autoware/trajectory/interpolator/akima_spline.hpp" #include #include #include -namespace autoware::motion_utils::trajectory_container::interpolator +namespace autoware::trajectory::interpolator { void AkimaSpline::compute_parameters( @@ -89,4 +89,4 @@ double AkimaSpline::compute_second_derivative_impl(const double & s) const return 2 * c_[i] + 6 * d_[i] * dx; } -} // namespace autoware::motion_utils::trajectory_container::interpolator +} // namespace autoware::trajectory::interpolator diff --git a/common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp b/common/autoware_trajectory/src/interpolator/cubic_spline.cpp similarity index 91% rename from common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp rename to common/autoware_trajectory/src/interpolator/cubic_spline.cpp index e464fc7cd0511..378233e799902 100644 --- a/common/autoware_motion_utils/src/trajectory_container/interpolator/cubic_spline.cpp +++ b/common/autoware_trajectory/src/interpolator/cubic_spline.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/motion_utils/trajectory_container/interpolator/cubic_spline.hpp" +#include "autoware/trajectory/interpolator/cubic_spline.hpp" #include -namespace autoware::motion_utils::trajectory_container::interpolator +namespace autoware::trajectory::interpolator { void CubicSpline::compute_parameters( @@ -90,4 +90,4 @@ double CubicSpline::compute_second_derivative_impl(const double & s) const return 2 * c_(i) + 6 * d_(i) * dx; } -} // namespace autoware::motion_utils::trajectory_container::interpolator +} // namespace autoware::trajectory::interpolator diff --git a/common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp b/common/autoware_trajectory/src/interpolator/linear.cpp similarity index 86% rename from common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp rename to common/autoware_trajectory/src/interpolator/linear.cpp index fb714b208772c..7d75b759050b8 100644 --- a/common/autoware_motion_utils/src/trajectory_container/interpolator/linear.cpp +++ b/common/autoware_trajectory/src/interpolator/linear.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/motion_utils/trajectory_container/interpolator/linear.hpp" +#include "autoware/trajectory/interpolator/linear.hpp" #include #include -namespace autoware::motion_utils::trajectory_container::interpolator +namespace autoware::trajectory::interpolator { void Linear::build_impl(const std::vector & bases, const std::vector & values) @@ -57,4 +57,4 @@ size_t Linear::minimum_required_points() const { return 2; } -} // namespace autoware::motion_utils::trajectory_container::interpolator +} // namespace autoware::trajectory::interpolator diff --git a/common/autoware_motion_utils/src/trajectory_container/interpolator/spherical_linear.cpp b/common/autoware_trajectory/src/interpolator/spherical_linear.cpp similarity index 87% rename from common/autoware_motion_utils/src/trajectory_container/interpolator/spherical_linear.cpp rename to common/autoware_trajectory/src/interpolator/spherical_linear.cpp index 33aaea718376d..ed3ba62031185 100644 --- a/common/autoware_motion_utils/src/trajectory_container/interpolator/spherical_linear.cpp +++ b/common/autoware_trajectory/src/interpolator/spherical_linear.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/motion_utils/trajectory_container/interpolator/spherical_linear.hpp" +#include "autoware/trajectory/interpolator/spherical_linear.hpp" #include #include -namespace autoware::motion_utils::trajectory_container::interpolator +namespace autoware::trajectory::interpolator { void SphericalLinear::build_impl( @@ -61,4 +61,4 @@ size_t SphericalLinear::minimum_required_points() const { return 2; } -} // namespace autoware::motion_utils::trajectory_container::interpolator +} // namespace autoware::trajectory::interpolator diff --git a/common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_path_point.cpp b/common/autoware_trajectory/src/path_point.cpp similarity index 77% rename from common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_path_point.cpp rename to common/autoware_trajectory/src/path_point.cpp index 8979e17b5b9e1..af1e9286b533d 100644 --- a/common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_path_point.cpp +++ b/common/autoware_trajectory/src/path_point.cpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point.hpp" +#include "autoware/trajectory/path_point.hpp" -#include "autoware/motion_utils/trajectory_container/trajectory/detail/utils.hpp" +#include "autoware/trajectory/detail/utils.hpp" #include -namespace autoware::motion_utils::trajectory_container::trajectory +namespace autoware::trajectory { using PointType = autoware_planning_msgs::msg::PathPoint; -bool TrajectoryContainer::build(const std::vector & points) +bool Trajectory::build(const std::vector & points) { std::vector poses; std::vector longitudinal_velocity_mps_values; @@ -39,7 +39,7 @@ bool TrajectoryContainer::build(const std::vector & points bool is_valid = true; - is_valid &= TrajectoryContainer::build(poses); + 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); @@ -47,10 +47,10 @@ bool TrajectoryContainer::build(const std::vector & points return is_valid; } -PointType TrajectoryContainer::compute(double s) const +PointType Trajectory::compute(double s) const { PointType result; - result.pose = TrajectoryContainer::compute(s); + 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)); @@ -58,7 +58,7 @@ PointType TrajectoryContainer::compute(double s) const return result; } -std::vector TrajectoryContainer::restore(const size_t & min_points) const +std::vector Trajectory::restore(const size_t & min_points) const { auto get_bases = [](const auto & interpolated_array) { auto [bases, values] = interpolated_array.get_data(); @@ -76,7 +76,7 @@ std::vector TrajectoryContainer::restore(const size_t & mi points.reserve(bases.size()); for (const auto & s : bases) { PointType p; - p.pose = TrajectoryContainer::compute(s); + p.pose = Trajectory::compute(s); p.longitudinal_velocity_mps = static_cast(this->longitudinal_velocity_mps.compute(s)); p.lateral_velocity_mps = static_cast(this->lateral_velocity_mps.compute(s)); p.heading_rate_rps = static_cast(this->heading_rate_rps.compute(s)); @@ -86,4 +86,4 @@ std::vector TrajectoryContainer::restore(const size_t & mi return points; } -} // namespace autoware::motion_utils::trajectory_container::trajectory +} // namespace autoware::trajectory diff --git a/common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_path_point_with_lane_id.cpp b/common/autoware_trajectory/src/path_point_with_lane_id.cpp similarity index 76% rename from common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_path_point_with_lane_id.cpp rename to common/autoware_trajectory/src/path_point_with_lane_id.cpp index 7bfe6ec788db1..9906c577d45c7 100644 --- a/common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_path_point_with_lane_id.cpp +++ b/common/autoware_trajectory/src/path_point_with_lane_id.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/motion_utils/trajectory_container/trajectory/trajectory_path_point_with_lane_id.hpp" +#include "autoware/trajectory/path_point_with_lane_id.hpp" -#include "autoware/motion_utils/trajectory_container/trajectory/detail/utils.hpp" +#include "autoware/trajectory/detail/utils.hpp" -namespace autoware::motion_utils::trajectory_container::trajectory +namespace autoware::trajectory { using PointType = tier4_planning_msgs::msg::PathPointWithLaneId; -bool TrajectoryContainer::build(const std::vector & points) +bool Trajectory::build(const std::vector & points) { std::vector path_points; std::vector> lane_ids_values; @@ -36,7 +36,7 @@ bool TrajectoryContainer::build(const std::vector & points return is_valid; } -PointType TrajectoryContainer::compute(double s) const +PointType Trajectory::compute(double s) const { PointType result; result.point = BaseClass::compute(s); @@ -45,7 +45,7 @@ PointType TrajectoryContainer::compute(double s) const return result; } -std::vector TrajectoryContainer::restore(const size_t & min_points) const +std::vector Trajectory::restore(const size_t & min_points) const { auto get_bases = [](const auto & interpolated_array) { auto [bases, values] = interpolated_array.get_data(); @@ -70,4 +70,4 @@ std::vector TrajectoryContainer::restore(const size_t & mi return points; } -} // namespace autoware::motion_utils::trajectory_container::trajectory +} // namespace autoware::trajectory diff --git a/common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_point.cpp b/common/autoware_trajectory/src/point.cpp similarity index 71% rename from common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_point.cpp rename to common/autoware_trajectory/src/point.cpp index 324853024466a..c7b9a6ab74a38 100644 --- a/common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_point.cpp +++ b/common/autoware_trajectory/src/point.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/motion_utils/trajectory_container/trajectory/trajectory_point.hpp" +#include "autoware/trajectory/point.hpp" -#include "autoware/motion_utils/trajectory_container/trajectory/detail/utils.hpp" +#include "autoware/trajectory/detail/utils.hpp" #include #include @@ -23,12 +23,12 @@ #include #include -namespace autoware::motion_utils::trajectory_container::trajectory +namespace autoware::trajectory { using PointType = geometry_msgs::msg::Point; -bool TrajectoryContainer::build(const std::vector & points) +bool Trajectory::build(const std::vector & points) { std::vector xs; std::vector ys; @@ -59,22 +59,22 @@ bool TrajectoryContainer::build(const std::vector & points return is_valid; } -double TrajectoryContainer::clamp(const double & s, bool show_warning) const +double Trajectory::clamp(const double & s, bool show_warning) const { if ((s < 0 || s > length()) && show_warning) { RCLCPP_WARN( - rclcpp::get_logger("TrajectoryContainer"), - "The arc length %f is out of the trajectory length %f", s, length()); + 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_; } -double TrajectoryContainer::length() const +double Trajectory::length() const { return end_ - start_; } -PointType TrajectoryContainer::compute(double s) const +PointType Trajectory::compute(double s) const { s = clamp(s, true); PointType result; @@ -84,7 +84,7 @@ PointType TrajectoryContainer::compute(double s) const return result; } -double TrajectoryContainer::azimuth(double s) const +double Trajectory::azimuth(double s) const { s = clamp(s, true); double dx = x_interpolator_->compute_first_derivative(s); @@ -92,14 +92,14 @@ double TrajectoryContainer::azimuth(double s) const return std::atan2(dy, dx); } -double TrajectoryContainer::elevation(double s) const +double Trajectory::elevation(double s) const { s = clamp(s, true); double dz = z_interpolator_->compute_first_derivative(s); return std::atan2(dz, 1.0); } -double TrajectoryContainer::curvature(double s) const +double Trajectory::curvature(double s) const { s = clamp(s, true); double dx = x_interpolator_->compute_first_derivative(s); @@ -109,10 +109,10 @@ double TrajectoryContainer::curvature(double s) const return std::abs(dx * ddy - dy * ddx) / std::pow(dx * dx + dy * dy, 1.5); } -std::vector TrajectoryContainer::restore(const size_t & min_points) const +std::vector Trajectory::restore(const size_t & min_points) const { auto bases = detail::crop_bases(bases_, start_, end_); - bases = detail::fill_bases(bases, static_cast(min_points)); + bases = detail::fill_bases(bases, min_points); std::vector points; points.reserve(bases.size()); for (const auto & s : bases) { @@ -125,10 +125,10 @@ std::vector TrajectoryContainer::restore(const size_t & mi return points; } -void TrajectoryContainer::crop(const double & start, const double & length) +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::motion_utils::trajectory_container::trajectory +} // namespace autoware::trajectory diff --git a/common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_pose.cpp b/common/autoware_trajectory/src/pose.cpp similarity index 83% rename from common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_pose.cpp rename to common/autoware_trajectory/src/pose.cpp index 0c02f09e8ac3c..14806bb4701ae 100644 --- a/common/autoware_motion_utils/src/trajectory_container/trajectory/trajectory_pose.cpp +++ b/common/autoware_trajectory/src/pose.cpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/motion_utils/trajectory_container/trajectory/trajectory_pose.hpp" +#include "autoware/trajectory/pose.hpp" #include #include #include -namespace autoware::motion_utils::trajectory_container::trajectory +namespace autoware::trajectory { using PointType = geometry_msgs::msg::Pose; -bool TrajectoryContainer::build(const std::vector & points) +bool Trajectory::build(const std::vector & points) { std::vector path_points; std::vector orientations; @@ -41,7 +41,7 @@ bool TrajectoryContainer::build(const std::vector & points return is_valid; } -PointType TrajectoryContainer::compute(double s) const +PointType Trajectory::compute(double s) const { PointType result; result.position = BaseClass::compute(s); @@ -50,7 +50,7 @@ PointType TrajectoryContainer::compute(double s) const return result; } -void TrajectoryContainer::align_orientation_with_trajectory_direction() +void Trajectory::align_orientation_with_trajectory_direction() { std::vector aligned_orientations; for (const auto & s : bases_) { @@ -84,10 +84,10 @@ void TrajectoryContainer::align_orientation_with_trajectory_direction orientation_interpolator_->build(bases_, aligned_orientations); } -std::vector TrajectoryContainer::restore(const size_t & min_points) const +std::vector Trajectory::restore(const size_t & min_points) const { auto bases = detail::crop_bases(bases_, start_, end_); - bases = detail::fill_bases(bases, static_cast(min_points)); + bases = detail::fill_bases(bases, min_points); std::vector points; points.reserve(bases.size()); for (const auto & s : bases) { @@ -99,4 +99,4 @@ std::vector TrajectoryContainer::restore(const size_t & mi return points; } -} // namespace autoware::motion_utils::trajectory_container::trajectory +} // namespace autoware::trajectory diff --git a/common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp b/common/autoware_trajectory/test/test_interpolator.cpp similarity index 65% rename from common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp rename to common/autoware_trajectory/test/test_interpolator.cpp index 0b6747adedcb7..d00d48f8076bf 100644 --- a/common/autoware_motion_utils/test/src/trajectory_container/test_interpolator.cpp +++ b/common/autoware_trajectory/test/test_interpolator.cpp @@ -12,9 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/motion_utils/trajectory_container/interpolator/spherical_linear.hpp" - -#include +#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 @@ -48,11 +51,10 @@ class TestInterpolator : public ::testing::Test }; using Interpolators = testing::Types< - autoware::motion_utils::trajectory_container::interpolator::CubicSpline, - autoware::motion_utils::trajectory_container::interpolator::AkimaSpline, - autoware::motion_utils::trajectory_container::interpolator::Linear, - autoware::motion_utils::trajectory_container::interpolator::NearestNeighbor, - autoware::motion_utils::trajectory_container::interpolator::Stairstep>; + 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, ); @@ -66,15 +68,11 @@ TYPED_TEST(TestInterpolator, compute) } // Instantiate test cases for all interpolators -template class TestInterpolator< - autoware::motion_utils::trajectory_container::interpolator::CubicSpline>; -template class TestInterpolator< - autoware::motion_utils::trajectory_container::interpolator::AkimaSpline>; -template class TestInterpolator; -template class TestInterpolator< - autoware::motion_utils::trajectory_container::interpolator::NearestNeighbor>; -template class TestInterpolator< - autoware::motion_utils::trajectory_container::interpolator::Stairstep>; +template class TestInterpolator; +template class TestInterpolator; +template class TestInterpolator; +template class TestInterpolator>; +template class TestInterpolator>; /* * Test SphericalLinear interpolator @@ -92,17 +90,16 @@ geometry_msgs::msg::Quaternion create_quaternion(double w, double x, double y, d TEST(TestSphericalLinearInterpolator, compute) { - using autoware::motion_utils::trajectory_container::interpolator::SphericalLinear; + 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::motion_utils::trajectory_container::interpolator::SphericalLinear::Builder() - .set_bases(bases) - .set_values(quaternions) - .build(); + auto interpolator = autoware::trajectory::interpolator::SphericalLinear::Builder() + .set_bases(bases) + .set_values(quaternions) + .build(); if (!interpolator) { FAIL(); diff --git a/common/autoware_motion_utils/test/src/trajectory_container/test_trajectory_container.cpp b/common/autoware_trajectory/test/test_trajectory_container.cpp similarity index 83% rename from common/autoware_motion_utils/test/src/trajectory_container/test_trajectory_container.cpp rename to common/autoware_trajectory/test/test_trajectory_container.cpp index e5c054ab9abf7..77115091e1ccb 100644 --- a/common/autoware_motion_utils/test/src/trajectory_container/test_trajectory_container.cpp +++ b/common/autoware_trajectory/test/test_trajectory_container.cpp @@ -11,14 +11,13 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "autoware/trajectory/path_point_with_lane_id.hpp" #include #include -using Trajectory = autoware::motion_utils::trajectory_container::trajectory::TrajectoryContainer< - tier4_planning_msgs::msg::PathPointWithLaneId>; +using Trajectory = autoware::trajectory::Trajectory; tier4_planning_msgs::msg::PathPointWithLaneId path_point_with_lane_id( double x, double y, uint8_t lane_id) @@ -47,7 +46,7 @@ TEST(TrajectoryCreatorTest, create) } } -class TrajectoryContainerTest : public ::testing::Test +class TrajectoryTest : public ::testing::Test { public: std::optional trajectory; @@ -66,7 +65,7 @@ class TrajectoryContainerTest : public ::testing::Test } }; -TEST_F(TrajectoryContainerTest, compute) +TEST_F(TrajectoryTest, compute) { double length = trajectory->length(); @@ -83,7 +82,7 @@ TEST_F(TrajectoryContainerTest, compute) EXPECT_EQ(1, point.lane_ids[0]); } -TEST_F(TrajectoryContainerTest, manipulate_velocity) +TEST_F(TrajectoryTest, manipulate_velocity) { trajectory->longitudinal_velocity_mps = 10.0; trajectory->longitudinal_velocity_mps(trajectory->length() / 3, 2.0 * trajectory->length() / 3) = @@ -97,41 +96,38 @@ TEST_F(TrajectoryContainerTest, manipulate_velocity) EXPECT_EQ(10.0, point3.point.longitudinal_velocity_mps); } -TEST_F(TrajectoryContainerTest, direction) +TEST_F(TrajectoryTest, direction) { double dir = trajectory->azimuth(0.0); EXPECT_LT(0, dir); EXPECT_LT(dir, M_PI / 2); } -TEST_F(TrajectoryContainerTest, curvature) +TEST_F(TrajectoryTest, curvature) { double curv = trajectory->curvature(0.0); EXPECT_LT(-1.0, curv); EXPECT_LT(curv, 1.0); } -TEST_F(TrajectoryContainerTest, restore) +TEST_F(TrajectoryTest, restore) { - using namespace autoware::motion_utils::trajectory_container::trajectory; // NOLINT + using autoware::trajectory::Trajectory; // NOLINT trajectory->longitudinal_velocity_mps(4.0, trajectory->length()) = 5.0; { - auto points = - static_cast &>(*trajectory).restore(0); + auto points = static_cast &>(*trajectory).restore(0); EXPECT_EQ(10, points.size()); } { - auto points = - static_cast &>(*trajectory).restore(0); + auto points = static_cast &>(*trajectory).restore(0); EXPECT_EQ(10, points.size()); } { auto points = - static_cast &>(*trajectory) - .restore(0); + static_cast &>(*trajectory).restore(0); EXPECT_EQ(11, points.size()); } @@ -141,7 +137,7 @@ TEST_F(TrajectoryContainerTest, restore) } } -TEST_F(TrajectoryContainerTest, crossed) +TEST_F(TrajectoryTest, crossed) { geometry_msgs::msg::Pose pose1; pose1.position.x = 0.0; @@ -157,7 +153,7 @@ TEST_F(TrajectoryContainerTest, crossed) EXPECT_LT(*crossed_point, trajectory->length()); } -TEST_F(TrajectoryContainerTest, closest) +TEST_F(TrajectoryTest, closest) { geometry_msgs::msg::Pose pose; pose.position.x = 5.0; @@ -174,7 +170,7 @@ TEST_F(TrajectoryContainerTest, closest) EXPECT_LT(distance, 3.0); } -TEST_F(TrajectoryContainerTest, crop) +TEST_F(TrajectoryTest, crop) { double length = trajectory->length();