Autoware Trajectory#

This package provides classes to manage/manipulate Trajectory.

Nomenclature#

TODO

Overview#

Interpolators#

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:43:62

  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:104:114

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

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:132:146

  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:187:196

  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:237:245

  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:286:295

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