Autoware Trajectory

This package provides classes to manage/manipulate Trajectory.

Overview

Interpolators

The Trajectory class provides mathematical continuous representation and object oriented interface for discrete array of following point types

• geometry_msgs::Point
• geometry_msgs::Pose
• autoware_planning_msgs::PathPoint
• autoware_planning_msgs::PathPointWithLaneId
• autoware_planning_msgs::TrajectoryPoint
• lanelet::ConstPoint3d

by interpolating the given underlying points. Once built, arbitrary point on the curve is continuously parametrized by a single s coordinate.

View in Drawio

Given bases and values, the builder internally executes interpolation and return the result in the form of expected<T, E>. If successful, it contains the interpolator object.

```cpp title=”./examples/example_readme.cpp:48:67” –8<– common/autoware_trajectory/examples/example_readme.cpp:48:67 –8<–

Otherwise it contains the error object representing the failure reason. In the below snippet, cubic spline interpolation fails because the number of input points is 3, which is below the `minimum_required_points() = 4` of `CubicSpline`.


```cpp title="./examples/example_readme.cpp:109:119"
--8<--
common/autoware_trajectory/examples/example_readme.cpp:109:119
--8<--

In such cases the result expected object contains InterpolationFailure type with an error message like “base size 3 is less than minimum required 4”.

Nomenclature

This section introduces strict definition of several words used in this package to clarify the description of API and help the developers understand and grasp the geometric meaning of algorithms.

Word	Meaning	Illustration
curve	curve is an oriented bounded curve denoted as (x(s), y(s), z(s)) with additional properties, parameterized by s (s = 0 at the start).	View in Drawio There are 5 underlying points $\mathrm{P0} = (0, 0, 0)$ $\mathrm{P1} = (1/ \sqrt{2}, 1/ \sqrt{2}, 0)$ $\mathrm{P2} = (1/ \sqrt{2}, 1+1/ \sqrt{2}, 0)$ $\mathrm{P3} = (2/ \sqrt{2}, 1+2/ \sqrt{2}, 0)$ $\mathrm{P4} = (2/ \sqrt{2} + 1/ \sqrt{6}, 1+2/ \sqrt{2} + 1 / \sqrt{3}, 1 / \sqrt{2})$ and the arc length between each interval is $1, 2, 1, 1$ respectively, so $\mathrm{start} = 0$ and $\mathrm{end} = 5$.
underlying	underlying points of a curve refers to the list of 3D points from which the curve was interpolated.
arc length[m]	arc length denotes the approximate 3D length of of a curve and is computed based on the discrete underlying points.
s[m]	s denotes the 3D arc length coordinate starting from the base point (mostly the start point) of the curve and a point is identified by trajectory(s). Due to this definition, the actual curve length and arc length have subtle difference as illustrated.	View in Drawio The point for $s = 0.5$ is the purple dot, but the curve length from $\mathrm{P0}$ to this point does not equal to $0.5$. The exact curve length is $\int \sqrt{(\frac{dx}{dt})^2 + (\frac{dy}{dt})^2 + (\frac{dz}{dt})^2} dt$, which cannot be obtained in an analytical closed form.
curvature	curvature is computed using only X-Y 2D coordinate. This is based on the normal and natural assumption that roads are flat. Mathematically, it asserts that Gaussian curvature of road is uniformly 0. The sign of curvature is positive if the center of turning circle is on the left side, otherwise negative.	View in Drawio

API

Interpolators

AkimaSpline requires at least 5 points to interpolate.

```cpp title=”./examples/example_readme.cpp:137:151” –8<– common/autoware_trajectory/examples/example_readme.cpp:137:151 –8<–

![akima_spline](./images/akima_spline.drawio.svg)
[View in Drawio]({{ drawio("/common/autoware_trajectory/images/akima_spline.drawio.svg") }})

`CubicSpline` requires at least **4** points to interpolate.


```cpp title="./examples/example_readme.cpp:192:201"
--8<--
common/autoware_trajectory/examples/example_readme.cpp:192:201
--8<--

View in Drawio

Linear requires at least 2 points to interpolate.

```cpp title=”./examples/example_readme.cpp:242:250” –8<– common/autoware_trajectory/examples/example_readme.cpp:242:250 –8<–

![linear](./images/linear.drawio.svg)
[View in Drawio]({{ drawio("/common/autoware_trajectory/images/linear.drawio.svg") }})

`StairStep` requires at least **2** points to interpolate.


```cpp title="./examples/example_readme.cpp:291:300"
--8<--
common/autoware_trajectory/examples/example_readme.cpp:291:300
--8<--

View in Drawio

Example Usage

This section describes Example Usage of Trajectory<autoware_planning_msgs::msg::PathPoint>

• Load Trajectory from point array

  #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

  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

  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

  double length = trajectory->length();
  

• Set 3.0[m] ~ 5.0[m] part of velocity to 0.0

  trajectory->longitudinal_velocity_mps(3.0, 5.0) = 0.0;
  

• Crop Trajectory from 1.0[m] to 2.0[m]

  trajectory->crop(1.0, 2.0);
  

• Restore points

  std::vector<autoware_planning_msgs::msg::PathPoint> points = trajectory->restore();
  

Changelog for package autoware_trajectory

1.0.0 (2025-03-31)

• feat(trajectory): remove default ctor and collect default setting in Builder (#287)
• fix(autoware_trajectory): fix linking issue with pybind11, and use non-deprecated tf2 headers (#316)
  • Fix linking issue with pybind11, and use non-deprecated tf2 headers
  • Use .hpp includes only
  • style(pre-commit): autofix
  • Remove redundant find_package(pybind11_vendor ...)
  • Undo whitespace change

  * Make pybind11 a test_depend ---------Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]\@users.noreply.github.com>

• Contributors: Mamoru Sobue, Shane Loretz

0.3.0 (2025-03-21)

• chore: fix versions in package.xml
• feat(trajectory): improve comment, use autoware_pyplot for examples (#282) Co-authored-by: Yutaka Kondo <<yutaka.kondo@youtalk.jp>>
• feat(autoware_trajectory): use move semantics and return expected<T, E> for propagating failure reason (#254) Co-authored-by: Yukinari Hisaki <<42021302+yhisaki@users.noreply.github.com>>
• refactor(autoware_trajectory): use nodiscard for mutables, fix reference to scalar type (#255)
  • doc(lanelet2_utils): fix invalid drawio link and update image
  • fix

  * fix precommit errors ---------Co-authored-by: Y.Hisaki <<yhisaki31@gmail.com>>

• feat(autoware_trajectory): add trajectory point (#233)
  • add TrajectoryPoint class to templates
  • add tests
  • add method to_point for TrajectoryPoint type
  • change name of test to avoid name collision
  • add missing items

  * rename example name for clarity ---------Co-authored-by: Y.Hisaki <<yhisaki31@gmail.com>>

• fix(autoware_trajectory): fix a bug of align_orientation_with_trajectory_direction (#234)
  • fix bug of align_orientation_with_trajectory_direction
  • fixed in a better way
  • reflect comments

  * revert unnecessary changes ---------

• feat(autoware_trajecotry): add a conversion function from point trajectory to pose trajectory (#207) feat(autoware_trajecotry): add conversion function from point trajectory to pose trajectory
• fix(autoware_trajectory): fix a bug of example file (#204)
• chore(autoware_trajectory): resolve clang-tidy warning of example file (#206)
• feat(autoware_trajectory): add curvature_utils (#205)
• feat: porting [autoware_trajectory]{.title-ref} from [autoware.universe]{.title-ref} to [autoware.core]{.title-ref} (#188)
  • add(autoware_trajectory): ported as follows (see below):
  • From [autoware.universe/common]{.title-ref} to [autoware.core/common]{.title-ref}

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

• Contributors: Junya Sasaki, Mamoru Sobue, Yukinari Hisaki, danielsanchezaran, mitsudome-r