feat(autoware_trajectory): move trajectory_container from autoware_mo…

…tion_utils to a new package (#9253) * create trajectory container package Signed-off-by: Y.Hisaki <[email protected]> * update Signed-off-by: Y.Hisaki <[email protected]> * update Signed-off-by: Y.Hisaki <[email protected]> * style(pre-commit): autofix * update codeowner Signed-off-by: Y.Hisaki <[email protected]> * update Signed-off-by: Y.Hisaki <[email protected]> * fix cmake Signed-off-by: Y.Hisaki <[email protected]> --------- Signed-off-by: Y.Hisaki <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Nov 7, 2024 · ca115d6 · ca115d6
1 parent 6c4de54
commit ca115d6
Show file tree

Hide file tree

Showing 40 changed files with 439 additions and 374 deletions.
diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
@@ -22,6 +22,7 @@ common/autoware_signal_processing/** [email protected] [email protected]
 common/autoware_test_utils/** [email protected] [email protected] [email protected] [email protected]
 common/autoware_testing/** [email protected] [email protected] [email protected] [email protected]
 common/autoware_time_utils/** [email protected] [email protected] [email protected]
+common/autoware_trajectory_container/** [email protected] [email protected] [email protected]
 common/autoware_universe_utils/** [email protected] [email protected] [email protected]
 common/autoware_vehicle_info_utils/** [email protected] [email protected] [email protected] [email protected]
 common/bag_time_manager_rviz_plugin/** [email protected]

diff --git 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/...autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp b/...autoware_motion_utils/include/autoware/motion_utils/trajectory_container/interpolator.hpp
diff --git a/...n/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory.hpp b/...n/autoware_motion_utils/include/autoware/motion_utils/trajectory_container/trajectory.hpp
diff --git a/common/autoware_motion_utils/package.xml b/common/autoware_motion_utils/package.xml
@@ -34,7 +34,6 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tier4_planning_msgs</depend>
   <depend>visualization_msgs</depend>
-  <!-- <depend>lanelet2_core</depend> -->
 
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>

diff --git a/common/autoware_trajectory/CMakeLists.txt 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
@@ -0,0 +1,64 @@
+# Autoware Trajectory
+
+This package provides classes to manage/manipulate Trajectory.
+
+## Example Usage
+
+This section describes Example Usage of `Trajectory<autoware_planning_msgs::msg::PathPoint>`
+
+- Load Trajectory from point array
+
+  ```cpp
+  #include "autoware/trajectory/path_point.hpp"
+
+  ...
+
+  std::vector<autoware_planning_msgs::msg::PathPoint> points = ... // Load points from somewhere
+
+  using autoware::trajectory::Trajectory;
+
+  std::optional<Trajectory<autoware_planning_msgs::msg::PathPoint>> trajectory =
+    Trajectory<autoware_planning_msgs::msg::PathPoint>::Builder{}
+      .build(points);
+  ```
+
+- You can also specify interpolation method
+
+  ```cpp
+  using autoware::trajectory::interpolator::CubicSpline;
+
+  std::optional<Trajectory<autoware_planning_msgs::msg::PathPoint>> trajectory =
+    Trajectory<autoware_planning_msgs::msg::PathPoint>::Builder{}
+      .set_xy_interpolator<CubicSpline>()  // 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<autoware_planning_msgs::msg::PathPoint> points = trajectory->restore();
+  ```
diff --git a/...n_utils/examples/example_interpolator.cpp → ...jectory/examples/example_interpolator.cpp b/...n_utils/examples/example_interpolator.cpp → ...jectory/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 <matplotlibcpp17/pyplot.h>
 
@@ -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<double> x;
     std::vector<double> 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<double> 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<double> x;
     std::vector<double> 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<double>::Builder{}.set_bases(bases).set_values(values).build();
     std::vector<double> 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<double>::Builder{}.set_bases(bases).set_values(values).build();
     std::vector<double> x;
     std::vector<double> y;

diff --git a/...xamples/example_trajectory_path_point.cpp → ...xamples/example_trajectory_path_point.cpp b/...xamples/example_trajectory_path_point.cpp → ...xamples/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 <autoware_planning_msgs/msg/path_point.hpp>
 #include <geometry_msgs/msg/point.hpp>
@@ -21,7 +21,7 @@
 
 #include <vector>
 
-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<autoware_planning_msgs::msg::PathPoint> 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<double> x;
@@ -57,8 +59,7 @@ int main()
     plt.scatter(Args(x, y), Kwargs("label"_a = "Original", "color"_a = "red"));
   }
 
-  auto trajectory =
-    TrajectoryContainer<autoware_planning_msgs::msg::PathPoint>::Builder{}.build(points);
+  auto trajectory = Trajectory<autoware_planning_msgs::msg::PathPoint>::Builder{}.build(points);
 
   if (!trajectory) {
     std::cerr << "Failed to build trajectory" << std::endl;

diff --git a/...ils/examples/example_trajectory_point.cpp → ...ory/examples/example_trajectory_point.cpp b/...ils/examples/example_trajectory_point.cpp → ...ory/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 <geometry_msgs/msg/point.hpp>
 
@@ -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<geometry_msgs::msg::Point>::Builder()
+  auto trajectory = Trajectory<geometry_msgs::msg::Point>::Builder()
                       .set_xy_interpolator<CubicSpline>()
                       .set_z_interpolator<CubicSpline>()
                       .build(points);

diff --git a/...tils/examples/example_trajectory_pose.cpp → ...tory/examples/example_trajectory_pose.cpp b/...tils/examples/example_trajectory_pose.cpp → ...tory/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 <geometry_msgs/msg/pose.hpp>
 
@@ -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<geometry_msgs::msg::Pose>::Builder{}.build(poses);
+  auto trajectory = Trajectory<geometry_msgs::msg::Pose>::Builder{}.build(poses);
 
   trajectory->align_orientation_with_trajectory_direction();