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.

interpolators 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.

./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.
  const CubicSpline & trajectory = result.value();

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

  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".

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);
  const auto ddy = trajectory.compute_second_derivative(x);

akima_spline View in Drawio

CubicSpline requires at least 4 points to interpolate.

./examples/example_readme.cpp:192:201

  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);
  const auto ddy = trajectory.compute_second_derivative(x);

cubic_spline View in Drawio

Linear requires at least 2 points to interpolate.

./examples/example_readme.cpp:242:250

  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);
  const auto ddy = trajectory.compute_second_derivative(x);

linear View in Drawio

StairStep requires at least 2 points to interpolate.

./examples/example_readme.cpp:291:300

  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);
  const auto ddy = trajectory.compute_second_derivative(x);

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