feat: porting autoware_trajectory from autoware.universe to `auto…

…ware.core` (#188)

* add(autoware_trajectory): ported as follows (see below):

  * From `autoware.universe/common` to `autoware.core/common`
  * The history can be traced via:
      https://github.com/sasakisasaki/autoware.universe/tree/02733e7b2932ad0d1c3c9c3a2818e2e4229f2e92/common/autoware_trajectory

Signed-off-by: Junya Sasaki <[email protected]>

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()

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();
+  ```

common/autoware_trajectory/examples/example_find_intervals.cpp

@@ -0,0 +1,109 @@
+// Copyright 2025 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/trajectory/path_point_with_lane_id.hpp"
+#include "autoware/trajectory/utils/find_intervals.hpp"
+
+#include <matplotlibcpp17/pyplot.h>
+
+#include <vector>
+
+using Trajectory =
+  autoware::trajectory::Trajectory<autoware_internal_planning_msgs::msg::PathPointWithLaneId>;
+
+autoware_internal::msg::PathPointWithLaneId path_point_with_lane_id(
+  double x, double y, uint8_t lane_id)
+{
+  autoware_internal::msg::PathPointWithLaneId point;
+  point.point.pose.position.x = x;
+  point.point.pose.position.y = y;
+  point.lane_ids.emplace_back(lane_id);
+  return point;
+}
+
+int main()
+{
+  pybind11::scoped_interpreter guard{};
+  auto plt = matplotlibcpp17::pyplot::import();
+
+  std::vector<autoware_internal::msg::PathPointWithLaneId> points{
+    path_point_with_lane_id(0.41, 0.69, 0), path_point_with_lane_id(0.66, 1.09, 0),
+    path_point_with_lane_id(0.93, 1.41, 0), path_point_with_lane_id(1.26, 1.71, 0),
+    path_point_with_lane_id(1.62, 1.90, 0), path_point_with_lane_id(1.96, 1.98, 0),
+    path_point_with_lane_id(2.48, 1.96, 1), path_point_with_lane_id(3.02, 1.87, 1),
+    path_point_with_lane_id(3.56, 1.82, 1), path_point_with_lane_id(4.14, 2.02, 1),
+    path_point_with_lane_id(4.56, 2.36, 1), path_point_with_lane_id(4.89, 2.72, 1),
+    path_point_with_lane_id(5.27, 3.15, 1), path_point_with_lane_id(5.71, 3.69, 1),
+    path_point_with_lane_id(6.09, 4.02, 0), path_point_with_lane_id(6.54, 4.16, 0),
+    path_point_with_lane_id(6.79, 3.92, 0), path_point_with_lane_id(7.11, 3.60, 0),
+    path_point_with_lane_id(7.42, 3.01, 0)};
+
+  auto trajectory = Trajectory::Builder{}.build(points);
+
+  if (!trajectory) {
+    return 1;
+  }
+
+  const auto intervals = autoware::trajectory::find_intervals(
+    *trajectory, [](const autoware_internal::msg::PathPointWithLaneId & point) {
+      return point.lane_ids[0] == 1;
+    });
+
+  std::vector<double> x_id0;
+  std::vector<double> y_id0;
+  std::vector<double> x_id1;
+  std::vector<double> y_id1;
+  std::vector<double> x_all;
+  std::vector<double> y_all;
+
+  for (const auto & point : points) {
+    if (point.lane_ids[0] == 0) {
+      x_id0.push_back(point.point.pose.position.x);
+      y_id0.push_back(point.point.pose.position.y);
+    } else {
+      x_id1.push_back(point.point.pose.position.x);
+      y_id1.push_back(point.point.pose.position.y);
+    }
+  }
+
+  for (double s = 0.0; s < trajectory->length(); s += 0.01) {
+    auto p = trajectory->compute(s);
+    x_all.push_back(p.point.pose.position.x);
+    y_all.push_back(p.point.pose.position.y);
+  }
+
+  plt.plot(Args(x_all, y_all), Kwargs("color"_a = "blue"));
+  plt.scatter(Args(x_id0, y_id0), Kwargs("color"_a = "blue", "label"_a = "lane_id = 0"));
+  plt.scatter(Args(x_id1, y_id1), Kwargs("color"_a = "red", "label"_a = "lane_id = 1"));
+
+  std::vector<double> x_cropped;
+  std::vector<double> y_cropped;
+
+  trajectory->crop(intervals[0].start, intervals[0].end - intervals[0].start);
+
+  for (double s = 0.0; s < trajectory->length(); s += 0.01) {
+    auto p = trajectory->compute(s);
+    x_cropped.push_back(p.point.pose.position.x);
+    y_cropped.push_back(p.point.pose.position.y);
+  }
+
+  plt.plot(
+    Args(x_cropped, y_cropped),
+    Kwargs("color"_a = "red", "label"_a = "interval between lane_id = 1"));
+
+  plt.legend();
+  plt.show();
+
+  return 0;
+}

common/autoware_trajectory/examples/example_interpolator.cpp

@@ -0,0 +1,115 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/trajectory/interpolator/akima_spline.hpp"
+#include "autoware/trajectory/interpolator/cubic_spline.hpp"
+#include "autoware/trajectory/interpolator/interpolator.hpp"
+#include "autoware/trajectory/interpolator/linear.hpp"
+#include "autoware/trajectory/interpolator/nearest_neighbor.hpp"
+#include "autoware/trajectory/interpolator/stairstep.hpp"
+
+#include <matplotlibcpp17/pyplot.h>
+
+#include <random>
+#include <vector>
+
+int main()
+{
+  pybind11::scoped_interpreter guard{};
+  auto plt = matplotlibcpp17::pyplot::import();
+
+  // create random values
+  std::vector<double> bases = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0};
+  std::vector<double> values;
+  std::random_device seed_gen;
+  std::mt19937 engine(seed_gen());
+  std::uniform_real_distribution<> dist(-1.0, 1.0);
+  for (size_t i = 0; i < bases.size(); ++i) {
+    values.push_back(dist(engine));
+  }
+  // Scatter Data
+  plt.scatter(Args(bases, values));
+
+  using autoware::trajectory::interpolator::InterpolatorInterface;
+  // Linear Interpolator
+  {
+    using autoware::trajectory::interpolator::Linear;
+    auto interpolator = *Linear::Builder{}.set_bases(bases).set_values(values).build();
+    std::vector<double> x;
+    std::vector<double> y;
+    for (double i = bases.front(); i < bases.back(); i += 0.01) {
+      x.push_back(i);
+      y.push_back(interpolator.compute(i));
+    }
+    plt.plot(Args(x, y), Kwargs("label"_a = "Linear"));
+  }
+
+  // AkimaSpline Interpolator
+  {
+    using autoware::trajectory::interpolator::AkimaSpline;
+
+    auto interpolator = *AkimaSpline::Builder{}.set_bases(bases).set_values(values).build();
+    std::vector<double> x;
+    std::vector<double> y;
+    for (double i = bases.front(); i < bases.back(); i += 0.01) {
+      x.push_back(i);
+      y.push_back(interpolator.compute(i));
+    }
+    plt.plot(Args(x, y), Kwargs("label"_a = "AkimaSpline"));
+  }
+
+  // CubicSpline Interpolator
+  {
+    using autoware::trajectory::interpolator::CubicSpline;
+    auto interpolator = *CubicSpline::Builder{}.set_bases(bases).set_values(values).build();
+    std::vector<double> x;
+    std::vector<double> y;
+    for (double i = bases.front(); i < bases.back(); i += 0.01) {
+      x.push_back(i);
+      y.push_back(interpolator.compute(i));
+    }
+    plt.plot(Args(x, y), Kwargs("label"_a = "CubicSpline"));
+  }
+
+  // NearestNeighbor Interpolator
+  {
+    using autoware::trajectory::interpolator::NearestNeighbor;
+    auto interpolator =
+      *NearestNeighbor<double>::Builder{}.set_bases(bases).set_values(values).build();
+    std::vector<double> x;
+    std::vector<double> y;
+    for (double i = bases.front(); i < bases.back(); i += 0.01) {
+      x.push_back(i);
+      y.push_back(interpolator.compute(i));
+    }
+    plt.plot(Args(x, y), Kwargs("label"_a = "NearestNeighbor"));
+  }
+
+  // Stairstep Interpolator
+  {
+    using autoware::trajectory::interpolator::Stairstep;
+    auto interpolator = *Stairstep<double>::Builder{}.set_bases(bases).set_values(values).build();
+    std::vector<double> x;
+    std::vector<double> y;
+    for (double i = bases.front(); i < bases.back(); i += 0.01) {
+      x.push_back(i);
+      y.push_back(interpolator.compute(i));
+    }
+    plt.plot(Args(x, y), Kwargs("label"_a = "Stairstep"));
+  }
+
+  plt.legend();
+  plt.show();
+  return 0;
+}