Autoware Trajectory#

This package provides classes to manage/manipulate Trajectory.

Overview#

Interpolators#

The interpolator class interpolates given bases and values. Following interpolators are implemented.

Linear
AkimaSpline
CubicSpline
NearestNeighbor
Stairstep

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

./examples/example_readme.cpp:48:67

  std::vector<double> xs = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0};
  std::vector<double> ys = {0.0 + noise(), 1.0 + noise(), 2.0 + noise(),
                            3.0 + noise(), 4.0 + noise(), 5.0 + noise()};

  // You need to use the `Builder` to try to construct the interpolator
  const tl::expected<CubicSpline, InterpolationFailure> result =
    CubicSpline::Builder()
      .set_bases(xs)   // set the base
      .set_values(ys)  // set the value
      .build();        // finally, call build()

  // The result may be `InterpolationFailure` type if the input is not suitable
  // for this interpolator
  if (!result) {
    return 0;
  }

  // If successful, the interpolator is available.
  // In this case the cubic spline coefficients are calculated.

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.

./examples/example_readme.cpp:109:119

  // You need to use the `Builder` to try to construct the interpolator
  const tl::expected<CubicSpline, InterpolationFailure> result =
    CubicSpline::Builder()
      .set_bases(xs)   // set the base
      .set_values(ys)  // set the value
      .build();        // finally, call build()

  // Access to the error reason
  if (!result) {
    // says "base size 3 is less than minimum required 4"
    std::cout << result.error().what << std::endl;

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

Trajectory class#

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.

./examples/example_readme.cpp:547:562

    pose(8.16, 6.52), pose(8.68, 8.45), pose(8.96, 8.96), pose(9.32, 9.36)};
  auto result = Trajectory<geometry_msgs::msg::Point>::Builder{}.build(underlying_points);
  if (!result) {
    std::cout << result.error().what << std::endl;
    return 0;
  }
  auto & trajectory = result.value();
  const auto s = trajectory.base_arange(0.05);  // like numpy.arange
  const auto C = trajectory.compute(s);
  const auto Cx = C | transform([&](const auto & p) { return p.x; }) | to<std::vector>();
  const auto Cy = C | transform([&](const auto & p) { return p.y; }) | to<std::vector>();
  const auto th = trajectory.azimuth(s);
  const auto cos_th =
    th | transform([&](const auto s) { return 1.5 * std::cos(s); }) | to<std::vector>();
  const auto sin_th =
    th | transform([&](const auto s) { return 1.5 * std::sin(s); }) | to<std::vector>();

overview_trajectory View in Drawio

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 $P 0 = (0, 0, 0)$ $P 1 = (1 / \sqrt{2}, 1 / \sqrt{2}, 0)$ $P 2 = (1 / \sqrt{2}, 1 + 1 / \sqrt{2}, 0)$ $P 3 = (2 / \sqrt{2}, 1 + 2 / \sqrt{2}, 0)$ $P 4 = (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 $start = 0$ and $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 $P 0$ to this point does not equal to $0.5$ . The exact curve length is $\int \sqrt{(\frac{d x}{d t})^{2} + (\frac{d y}{d t})^{2} + (\frac{d z}{d t})^{2}} d t$ , 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#

Class	method/function	description
Common Functions	`minimum_required_points()`	return the number of points required for each concrete interpolator
	`compute(double s) -> T`	compute the interpolated value at given base $s$ . $s$ is clamped to the underlying base range.
	`compute(vector<double> s) -> vector<T>`	compute the interpolated values at for each base values in $s$ .
	`compute_first_derivative(double s) -> double`	compute the first derivative of at given base $s$ . $s$ is clamped.
	`compute_second_derivative(double s) -> double`	compute the second derivative of at given base $s$ . $s$ is clamped.

AkimaSpline requires at least 5 points to interpolate.

./examples/example_readme.cpp:137:151

  std::vector<double> xs = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0};
  std::vector<double> ys = {0.0 + noise(), 1.0 + noise(), 2.0 + noise(),
                            3.0 + noise(), 4.0 + noise(), 5.0 + noise()};

  // You need to use the `Builder` to try to construct the interpolator
  const tl::expected<AkimaSpline, InterpolationFailure> result =
    AkimaSpline::Builder()
      .set_bases(xs)   // set the base
      .set_values(ys)  // set the value
      .build();        // finally, call build()
  const auto & trajectory = result.value();
  const auto x = trajectory.base_arange(0.05);
  const auto y = trajectory.compute(x);
  const auto dy = trajectory.compute_first_derivative(x);

akima_spline View in Drawio

CubicSpline requires at least 4 points to interpolate.

./examples/example_readme.cpp:192:201

  // You need to use the `Builder` to try to construct the interpolator
  const tl::expected<CubicSpline, InterpolationFailure> result =
    CubicSpline::Builder()
      .set_bases(xs)   // set the base
      .set_values(ys)  // set the value
      .build();        // finally, call build()
  const auto & trajectory = result.value();
  const auto x = trajectory.base_arange(0.05);
  const auto y = trajectory.compute(x);
  const auto dy = trajectory.compute_first_derivative(x);

cubic_spline View in Drawio

Linear requires at least 2 points to interpolate.

./examples/example_readme.cpp:242:250

  // You need to use the `Builder` to try to construct the interpolator
  const tl::expected<Linear, InterpolationFailure> result = Linear::Builder()
                                                              .set_bases(xs)   // set the base
                                                              .set_values(ys)  // set the value
                                                              .build();  // finally, call build()
  const auto & trajectory = result.value();
  const auto x = trajectory.base_arange(0.05);
  const auto y = trajectory.compute(x);
  const auto dy = trajectory.compute_first_derivative(x);

linear View in Drawio

StairStep requires at least 2 points to interpolate.

./examples/example_readme.cpp:291:300

  // You need to use the `Builder` to try to construct the interpolator
  const tl::expected<Stairstep<double>, InterpolationFailure> result =
    Stairstep<double>::Builder()
      .set_bases(xs)   // set the base
      .set_values(ys)  // set the value
      .build();        // finally, call build()
  const auto & trajectory = result.value();
  const auto x = trajectory.base_arange(0.05);
  const auto y = trajectory.compute(x);
  const auto dy = trajectory.compute_first_derivative(x);

stairstep View in Drawio

Trajectory class#

Several Trajectory<T> are defined in the following inheritance hierarchy according to the sub object relationships.

hierarchy View in Drawio

Each derived class in the diagram inherits the methods of all of its descending subclasses. For example, all of the classes have the methods like length(), curvature() in common.

Header/Class	method	description	illustration
`<autoware/trajectory/point.hpp>` `Trajectory<geometry_msgs::msg::Point>::Builder`	`Builder()`	set default interpolator setting as follows. `x, y`: Cubic `z`: Linear
	`set_xy_interpolator<InterpolatorType>()`	set custom interpolator for `x, y`.
	`set_z_interpolator<InterpolatorType>()`	set custom interpolator for `z`.
	`build(const vector<Point> &)`	return `expected<Trajectory<Point>, InterpolationFailure>` object.
`Trajectory<Point>`	`base_arange(const double step)`	return vector of `s` values starting from `start`, with the interval of `step`, including `end`. Thus the return value has at least the size of 2.
	`length()`	return the total `arc length` of the trajectory.	View in Drawio `length()` is $5.0$ because it computes the sum of the length of dotted lines.
	`azimuth(const double s)`	return the tangent angle at given `s` coordinate using `std::atan2`.	View in Drawio
	`curvature(const double s)`	return the `curvature` at given `s` coordinate.	See above

Example Usage#

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

You can also specify interpolation method to Builder{} before calling .build(points)

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

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