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:53:68
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;
}
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
// Give only 3 points for CubicSpline, which is infeasible
std::vector<double> xs = {0.0, 1.0, 2.0};
std::vector<double> ys = {0.0 + noise(), 1.0 + noise(), 2.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()
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
- [x]
geometry_msgs::Point - [x]
geometry_msgs::Pose - [x]
autoware_planning_msgs::PathPoint - [x]
autoware_planning_msgs::PathPointWithLaneId - [x]
autoware_planning_msgs::TrajectoryPoint
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:549:561
std::vector<geometry_msgs::msg::Point> underlying_points = {
pose(0.49, 0.59), pose(1.20, 2.56), pose(1.51, 3.17), pose(1.85, 3.76),
pose(2.60, 4.56), pose(3.61, 4.30), pose(3.95, 4.01), pose(4.90, 3.25),
pose(5.54, 3.10), pose(6.88, 3.54), pose(7.85, 4.93), pose(8.03, 5.73),
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); // cppcheck-suppress shadowVariable
const auto C = trajectory.compute(s);
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 |
k_points_minimum_dist_threshold |
This is a constant threshold that is used to check if two points or values are same and to avoid zero division. | |
almost-same |
The pair of two points \(P_{1}\) and \(P_{2}\), or the pair of two base values \(s_{1}\) and \(s_{2}\) are called almost-same if their distance or difference are less than k_points_minimum_dist_threshold |
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:142:152
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();
CubicSpline requires at least 4 points to interpolate.
./examples/example_readme.cpp:192:201
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()
Linear requires at least 2 points to interpolate.
./examples/example_readme.cpp:242:250
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<Linear, InterpolationFailure> result = Linear::Builder()
.set_bases(xs) // set the base
.set_values(ys) // set the value
.build(); // finally, call build()
StairStep requires at least 2 points to interpolate.
./examples/example_readme.cpp:291:300
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<Stairstep<double>, InterpolationFailure> result =
Stairstep<double>::Builder()
.set_bases(xs) // set the base
.set_values(ys) // set the value
.build(); // finally, call build()
Trajectory class#
Several Trajectory<T> are defined in the following inheritance hierarchy according to the sub object relationships.
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>
|
Builder() |
set default interpolator setting as follows.
|
|
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. |
||
|
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 signed curvature at given s coordinate following \(\sqrt{\dot{x}^2 + \dot{y}^2}^3 / (\dot{x}\ddot{y} - \dot{y}\ddot{x})\). |
See above | |
elevation(const double s) |
return the elevation angle at given s coordinate. |
||
get_underlying_base() |
return the vector of s values of current underlying points. |
||
<autoware/trajectory/pose.hpp>
|
Builder() |
set default interpolator setting in addition to that of Trajectory<Point>::Builder as follows.
|
|
set_orientation_interpolator<InterpolatorType>() |
set custom interpolator for orientation. |
||
build(const vector<Pose> &) |
return expected<Trajectory<Pose>, InterpolationFailure> object. |
||
|
derives all of the above methods of Trajectory<Point> |
||
align_orientation_with_trajectory_direction() |
update the underlying points so that their orientations match the azimuth() of interpolated curve. This is useful when the user gave only the position of Pose and created Trajectory object. |
 View in Drawio |
|
<autoware/trajectory/path_point.hpp>
|
Builder() |
set default interpolator setting in addition to that of Trajectory<Pose>::Builder as follows.
|
|
set_longitudinal_velocity_mps_interpolator<InterpolatorType>() |
set custom interpolator for longitudinal_velocity_mps. |
||
set_lateral_velocity_mps_interpolator<InterpolatorType>() |
set custom interpolator for lateral_velocity_mps. |
||
set_heading_rate_rps_interpolator<InterpolatorType>() |
set custom interpolator for heading_rate_rps. |
||
build(const vector<PathPoint> &) |
return expected<Trajectory<PathPoint>, InterpolationFailure> object. |
||
|
derives all of the above methods of Trajectory<Pose> |
||
longitudinal_velocity_mps() |
return reference to longitudinal_velocity_mps |
||
lateral_velocity_mps() |
return reference to lateral_velocity_mps |
||
heading_rate_rps_mps() |
return reference to heading_rate_rps |
||
<autoware/trajectory/path_point_with_lane_id.hpp>
|
Builder() |
set default interpolator setting in addition to that of Trajectory<PathPoint>::Builder as follows.
|
|
set_lane_ids_interpolator<InterpolatorType>() |
set custom interpolator for lane_ids. |
||
build(const vector<PathPointWithLaneId> &) |
return expected<Trajectory<PathPointWithLaneId>, InterpolationFailure> object. |
||
|
derives all of the above methods of Trajectory<PathPoint> |
||
lane_ids() |
return reference to lane_ids |
Utility functions#
| Header / function | description | detail |
|---|---|---|
<autoware/trajectory/utils/shift.hpp |
 View in Drawio This is the case where \(a_{\mathrm{max}}^{\mathrm{lat}} > a_{\mathrm{lim}}^{\mathrm{lat}}\) because newly 4 points are inserted in the shift interval. |
|
|
contains
|
Returns the shifted trajectory as well the s values indicating where the shift starts and completes on the new shifted trajectory. View in Drawio |
|
contains
|
 View in Drawio |
|
contains
|
|
|
Following formulation, return a shifted Trajectory object if the parameters are feasible, otherwise return Error object indicating error reason(i.e. \(T_j\) becomes negative, \(j\) becomes negative, etc.). |
For derivation, see formulation. The example code for this plot is found example |
<autoware/trajectory/utils/pretty_build.hpp> |
||
|
A convenient function that will almost surely succeed in constructing a Trajectory class unless the given points size is 0 or 1. Input points are interpolated to 3 points using Linear and to 4 points using Cubic so that it returns
default interpolators setting.You may need to call align_orientation_with_trajectory_direction if you did not give desired orientation. |
 View in Drawio |
<autoware/trajectory/utils/reference_path.hpp> |
||
|
A convenient function that generates Trajectory class from given Lanelets and specified length before/after given position. Input lanelet_sequence must be consecutive.If lanelet_sequence have self-intersection ego position gets ambiguous, so both current_lanelet and current_pose need to be given.forward/backward_length is measured in terms of lane coordinate from current_pose. Thus, if and only if the Lanelets contained waypoint, the output trajectory length does not equal to forward_length + backward_length. |
 View in Drawio |
Derivation of shift#
shift function laterally offsets given curve by \(l(s)\) in normal direction at each point following the lateral time-jerk profile as shown bellow.
Starting from the initial longitudinal velocity of \(v_{0}^{\mathrm{lon}}\) and longitudinal acceleration of \(a^{\mathrm{lon}}\), at each time \(t_{1}, \cdots, t_{7}\), the lateral offset \(l_{i}\) at the corresponding longitudinal position \(s_{i}\)(with the longitudinal velocity \(v_{i}\)) is expressed as follows.
\[
\begin{align}
& t_{1}: & s_{1} &= v^{\rm lon}_0 T_j + \frac{1}{2} a^{\rm lon} T_j^2, & v_{1} &= v^{\rm lon}_0 + a^{\rm lon} T_j, & l_{1} &= \frac{1}{6}jT_j^3 \\
& t_{2}: & s_{2} &= v^{\rm lon}_1 T_a + \frac{1}{2} a^{\rm lon} T_a^2, & v_{2} &= v^{\rm lon}_1 + a^{\rm lon} T_a, & l_{2} &= \frac{1}{6}j T_j^3 + \frac{1}{2} j T_a T_j^2 + \frac{1}{2} j T_a^2 T_j \\
& t_{3}: & s_{3} &= v^{\rm lon}_2 T_j + \frac{1}{2} a^{\rm lon} T_j^2, & v_{3} &= v^{\rm lon}_2 + a^{\rm lon} T_j, & l_{3} &= j T_j^3 + \frac{3}{2} j T_a T_j^2 + \frac{1}{2} j T_a^2 T_j \\
& t_{4}: & s_{4} &= v^{\rm lon}_3 T_v + \frac{1}{2} a^{\rm lon} T_v^2, & v_{4} &= v^{\rm lon}_3 + a^{\rm lon} T_v, & l_{4} &= j T_j^3 + \frac{3}{2} j T_a T_j^2 + \frac{1}{2} j T_a^2 T_j + j(T_a + T_j)T_j T_v \\
& t_{5}: & s_{5} &= v^{\rm lon}_4 T_j + \frac{1}{2} a^{\rm lon} T_j^2, & v_{5} &= v^{\rm lon}_4 + a^{\rm lon} T_j, & l_{5} &= \frac{11}{6} j T_j^3 + \frac{5}{2} j T_a T_j^2 + \frac{1}{2} j T_a^2 T_j + j(T_a + T_j)T_j T_v \\
& t_{6}: & s_{6} &= v^{\rm lon}_5 T_a + \frac{1}{2} a^{\rm lon} T_a^2, & v_{6} &= v^{\rm lon}_5 + a^{\rm lon} T_a, & l_{6} &= \frac{11}{6} j T_j^3 + 3 j T_a T_j^2 + j T_a^2 T_j + j(T_a + T_j)T_j T_v \\
& t_{7}: & s_{7} &= v^{\rm lon}_6 T_j + \frac{1}{2} a^{\rm lon} T_j^2, & v_{7} &= v^{\rm lon}_6 + a^{\rm lon} T_j, & l_{7} &= 2 j T_j^3 + 3 j T_a T_j^2 + j T_a^2 T_j + j(T_a + T_j)T_j T_v
\end{align}
\]
Given following inputs,
- desired longitudinal distance \(L_{\mathrm{lon}}\)
- desired lateral shift distance \(L\)
- longitudinal initial velocity \(v_{\mathrm{lon}}\)
- longitudinal acceleration \(a_{\mathrm{lon}}\)
- lateral acceleration limit \(a_{\mathrm{lim}}^{\mathrm{lat}}\)
shift internally computes
- total time of shift \(T_{\mathrm{total}}\)
- maximum lateral acceleration \(a_{\mathrm{max}}^{\mathrm{lat}}\) during the shift
- constant/zero jerk time \(T_{\mathrm{j}}, T_{\mathrm{a}}\) respectively
- required jerk \(j\)
as following
\[
\begin{align}
T_{\mathrm{total}} &= \text{(time to travel $L_{\mathrm{lon}}$ from $v_{\mathrm{lon}}$ under $a_{\mathrm{lon}}$)} \\
a_{\rm max}^{\rm lat} &= \frac{8L}{T_{\rm total}^2} \\
T_j&=\frac{T_{\rm total}}{2} - \frac{2L}{a_{\rm lim}^{\rm lat} T_{\rm total}}\\
T_a&=\frac{4L}{a_{\rm lim}^{\rm lat} T_{\rm total}} - \frac{T_{\rm total}}{2}\\
j&=\frac{2a_{\rm lim} ^{\mathrm{lat} 2}T_{\rm total}}{a_{\rm lim}^{\rm lat} T_{\rm total}^2-4L}
\end{align}
\]
to obtain \((s_{i}, l_{i})\) respectively. Note that \(l_{7} == L\).
If \(a_{\mathrm{max}}^{\mathrm{lat}} > a_{\mathrm{lim}}^{\mathrm{lat}}\), then \(l_{i}\) is simply
\[
\begin{align}
t &= \frac{T_{\mathrm{total}}}{4} \\
s_{1} &= v_{\mathrm{lon}}t + \frac{1}{2} a_{\mathrm{lon}}t^2, & l_{1} &= \frac{L}{12} \\
s_{2} &= s_{1} + 2 (v_{0} + a_{\mathrm{lon}}t)t + 2a_{\mathrm{lon}}t^2, & l_{2} &= \frac{11L}{12} \\
s_{3} &= L_{\mathrm{lon}}, & l_{3} &= L
\end{align}
\]
Example Usage#
pretty_build#
pretty_build is a convenient wrapper tailored for most Autoware Planning/Control component, which will never fail to interpolate unless the given points size is 0 or 1.
./examples/example_pretty_build.cpp:93:97
auto trajectory_opt = autoware::experimental::trajectory::pretty_build(points);
if (!trajectory_opt) {
return 1;
}
auto & trajectory = trajectory_opt.value();
use custom Interpolator#
You can also specify interpolation method to Builder{} before calling .build(points)
using autoware::experimental::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);
crop/shorten Trajectory#
trajectory->crop(1.0, 2.0);
shift Trajectory#
./examples/example_shift.cpp:97:117
// six points
const double longitudinal_velocity = 2.77;
const double lateral_jerk = 1.0;
const double lateral_shift = 2.5;
const double lateral_acc_limit = 5.0;
const double longitudinal_dist = autoware::motion_utils::calc_longitudinal_dist_from_jerk(
lateral_shift, lateral_jerk, longitudinal_velocity);
const auto start_s = 3.0;
const ShiftInterval shift_interval{start_s, start_s + longitudinal_dist, lateral_shift};
const ShiftParameters shift_parameter{
longitudinal_velocity,
lateral_acc_limit,
};
auto shifted_trajectory_info =
autoware::experimental::trajectory::shift(*trajectory, shift_interval, shift_parameter);
if (!shifted_trajectory_info) {
std::cout << shifted_trajectory_info.error().what << std::endl;
return 1;
}
Insert and set velocity profile#
Set 3.0[m] ~ 5.0[m] part of velocity to 0.0
trajectory->longitudinal_velocity_mps(3.0, 5.0) = 0.0;
Restore points#
std::vector<autoware_planning_msgs::msg::PathPoint> points = trajectory->restore();
Generate reference path from Lanelets#
./examples/example_reference_path.cpp:74:80
const double forward_length = 40;
const double backward_length = 0.0;
const auto reference_path_opt = trajectory::build_reference_path(
lanelet_sequence, current_lanelet, ego_pose, lanelet_map_ptr, routing_graph, traffic_rules,
forward_length, backward_length);
if (reference_path_opt) {
const auto & reference_path = reference_path_opt.value();
