Skip to content

Commit affdbbd

Browse files
soblinsoblin
authored
feat(trajectory): add API documentation for trajectory class, add some utillity (#295)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent 5a56426 commit affdbbd

File tree

7 files changed

+593
-39
lines changed

7 files changed

+593
-39
lines changed

Diff for: ‎common/autoware_trajectory/README.md

+49-38
Original file line numberDiff line numberDiff line change
@@ -6,21 +6,18 @@ This package provides classes to manage/manipulate Trajectory.
66

77
### Interpolators
88

9-
The _Trajectory_ class provides mathematical continuous representation and object oriented interface for discrete array of following point types
9+
The interpolator class interpolates given `bases` and `values`. Following interpolators are implemented.
1010

11-
- [x] `geometry_msgs::Point`
12-
- [x] `geometry_msgs::Pose`
13-
- [x] `autoware_planning_msgs::PathPoint`
14-
- [x] `autoware_planning_msgs::PathPointWithLaneId`
15-
- [x] `autoware_planning_msgs::TrajectoryPoint`
16-
- [ ] `lanelet::ConstPoint3d`
17-
18-
by interpolating the given _underlying_ points. Once built, arbitrary point on the curve is continuously parametrized by a single `s` coordinate.
11+
- Linear
12+
- AkimaSpline
13+
- CubicSpline
14+
- NearestNeighbor
15+
- Stairstep
1916

20-
![interpolators](./images/interpolators.drawio.svg)
21-
[View in Drawio]({{ drawio("/common/autoware_trajectory/images/interpolators.drawio.svg") }})
17+
![interpolators](./images/overview/interpolators.drawio.svg)
18+
[View in Drawio]({{ drawio("/common/autoware_trajectory/images/overview/interpolators.drawio.svg") }})
2219

23-
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.
20+
The builder internally executes interpolation and return the result in the form of `expected<T, E>`. If successful, it contains the interpolator object.
2421

2522
```cpp title="./examples/example_readme.cpp:48:67"
2623
--8<--
@@ -38,6 +35,28 @@ common/autoware_trajectory/examples/example_readme.cpp:109:119
3835

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

38+
### Trajectory class
39+
40+
The _Trajectory_ class provides mathematical continuous representation and object oriented interface for discrete array of following point types
41+
42+
- [x] `geometry_msgs::Point`
43+
- [x] `geometry_msgs::Pose`
44+
- [x] `autoware_planning_msgs::PathPoint`
45+
- [x] `autoware_planning_msgs::PathPointWithLaneId`
46+
- [x] `autoware_planning_msgs::TrajectoryPoint`
47+
- [ ] `lanelet::ConstPoint3d`
48+
49+
by interpolating the given _underlying_ points. Once built, arbitrary point on the curve is continuously parametrized by a single `s` coordinate.
50+
51+
```cpp title="./examples/example_readme.cpp:547:562"
52+
--8<--
53+
common/autoware_trajectory/examples/example_readme.cpp:547:562
54+
--8<--
55+
```
56+
57+
![overview_trajectory](./images/overview/trajectory.drawio.svg)
58+
[View in Drawio]({{ drawio("/common/autoware_trajectory/images/overview/trajectory.drawio.svg") }})
59+
4160
## Nomenclature
4261

4362
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.
@@ -106,27 +125,31 @@ common/autoware_trajectory/examples/example_readme.cpp:291:300
106125
![stairstep](./images/stairstep.drawio.svg)
107126
[View in Drawio]({{ drawio("/common/autoware_trajectory/images/stairstep.drawio.svg") }})
108127

109-
## Example Usage
128+
### Trajectory class
110129

111-
This section describes Example Usage of `Trajectory<autoware_planning_msgs::msg::PathPoint>`
130+
Several `Trajectory<T>` are defined in the following inheritance hierarchy according to the sub object relationships.
112131

113-
- Load Trajectory from point array
132+
![hierarchy](./images/nomenclature/trajectory_hierarchy.drawio.svg)
133+
[View in Drawio]({{ drawio("/common/autoware_trajectory/images/nomenclature/trajectory_hierarchy.drawio.svg") }})
114134

115-
```cpp
116-
#include "autoware/trajectory/path_point.hpp"
117-
118-
...
135+
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.
119136

120-
std::vector<autoware_planning_msgs::msg::PathPoint> points = ... // Load points from somewhere
137+
| Header/Class | method | description | illustration |
138+
| ------------------------------------------------------------------------------------------------------- | ----------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
139+
| `<autoware/trajectory/point.hpp>`<br><ul><li>`Trajectory<geometry_msgs::msg::Point>::Builder`</li></ul> | `Builder()` | set default interpolator setting as follows.<br><ul><li>`x, y`: Cubic</li><li>`z`: Linear</li></ul> | |
140+
| | `set_xy_interpolator<InterpolatorType>()` | set custom interpolator for `x, y`. | |
141+
| | `set_z_interpolator<InterpolatorType>()` | set custom interpolator for `z`. | |
142+
| | `build(const vector<Point> &)` | return `expected<Trajectory<Point>, InterpolationFailure>` object. | |
143+
| <ul><li>`Trajectory<Point>`</li></ul> | `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. | |
144+
| | `length()` | return the total `arc length` of the trajectory. | ![curve](./images/nomenclature/curve.drawio.svg)<br>[View in Drawio]({{ drawio("/common/autoware_trajectory/images/nomenclature/curve.drawio.svg") }})<br>`length()` is $5.0$ because it computes the sum of the length of dotted lines. |
145+
| | `azimuth(const double s)` | return the tangent angle at given `s` coordinate using `std::atan2`. | ![azimuth_angle](./images/overview/trajectory.drawio.svg)<br>[View in Drawio]({{ drawio("/common/autoware_trajectory/images/overview/trajectory.drawio.svg") }}) |
146+
| | `curvature(const double s)` | return the `curvature` at given `s` coordinate. | See above |
121147

122-
using autoware::trajectory::Trajectory;
148+
## Example Usage
123149

124-
std::optional<Trajectory<autoware_planning_msgs::msg::PathPoint>> trajectory =
125-
Trajectory<autoware_planning_msgs::msg::PathPoint>::Builder{}
126-
.build(points);
127-
```
150+
This section describes Example Usage of `Trajectory<autoware_planning_msgs::msg::PathPoint>`
128151

129-
- You can also specify interpolation method
152+
- You can also specify interpolation method to `Builder{}` before calling `.build(points)`
130153

131154
```cpp
132155
using autoware::trajectory::interpolator::CubicSpline;
@@ -137,18 +160,6 @@ This section describes Example Usage of `Trajectory<autoware_planning_msgs::msg:
137160
.build(points);
138161
```
139162

140-
- Access point on Trajectory
141-
142-
```cpp
143-
autoware_planning_msgs::msg::PathPoint point = trajectory->compute(1.0); // Get point at s=0.0. s is distance from start point on Trajectory.
144-
```
145-
146-
- Get length of Trajectory
147-
148-
```cpp
149-
double length = trajectory->length();
150-
```
151-
152163
- Set 3.0[m] ~ 5.0[m] part of velocity to 0.0
153164

154165
```cpp

Diff for: ‎common/autoware_trajectory/examples/example_readme.cpp

+69-1
Original file line numberDiff line numberDiff line change
@@ -521,6 +521,73 @@ int main_curvature()
521521
return 0;
522522
}
523523

524+
int main_trajectory_overview()
525+
{
526+
using autoware::trajectory::Trajectory;
527+
using ranges::to;
528+
using ranges::views::stride;
529+
using ranges::views::transform;
530+
531+
auto plt = autoware::pyplot::import();
532+
auto [fig, axes] = plt.subplots(1, 2);
533+
auto &ax = axes[0], ax1 = axes[1];
534+
535+
auto pose = [](double x, double y) -> geometry_msgs::msg::Point {
536+
geometry_msgs::msg::Point p;
537+
p.x = x;
538+
p.y = y;
539+
p.z = 0.0;
540+
return p;
541+
};
542+
543+
std::vector<geometry_msgs::msg::Point> underlying_points = {
544+
pose(0.49, 0.59), pose(1.20, 2.56), pose(1.51, 3.17), pose(1.85, 3.76),
545+
pose(2.60, 4.56), pose(3.61, 4.30), pose(3.95, 4.01), pose(4.90, 3.25),
546+
pose(5.54, 3.10), pose(6.88, 3.54), pose(7.85, 4.93), pose(8.03, 5.73),
547+
pose(8.16, 6.52), pose(8.68, 8.45), pose(8.96, 8.96), pose(9.32, 9.36)};
548+
auto result = Trajectory<geometry_msgs::msg::Point>::Builder{}.build(underlying_points);
549+
if (!result) {
550+
std::cout << result.error().what << std::endl;
551+
return 0;
552+
}
553+
auto & trajectory = result.value();
554+
const auto s = trajectory.base_arange(0.05); // like numpy.arange
555+
const auto C = trajectory.compute(s);
556+
const auto Cx = C | transform([&](const auto & p) { return p.x; }) | to<std::vector>();
557+
const auto Cy = C | transform([&](const auto & p) { return p.y; }) | to<std::vector>();
558+
const auto th = trajectory.azimuth(s);
559+
const auto cos_th =
560+
th | transform([&](const auto s) { return 1.5 * std::cos(s); }) | to<std::vector>();
561+
const auto sin_th =
562+
th | transform([&](const auto s) { return 1.5 * std::sin(s); }) | to<std::vector>();
563+
ax.plot(Args(Cx, Cy), Kwargs("color"_a = "purple", "label"_a = "Point interpolation"));
564+
ax.quiver(
565+
Args(
566+
Cx | stride(10) | to<std::vector>(), Cy | stride(10) | to<std::vector>(),
567+
cos_th | stride(10) | to<std::vector>(), sin_th | stride(10) | to<std::vector>()),
568+
Kwargs("color"_a = "green", "label"_a = "azimuth", "alpha"_a = 0.5));
569+
570+
ax1.plot(Args(s, trajectory.curvature(s)), Kwargs("color"_a = "purple", "label"_a = "curvature"));
571+
572+
const auto points_x = underlying_points |
573+
ranges::views::transform([&](const auto & p) { return p.x; }) |
574+
ranges::to<std::vector>();
575+
const auto points_y = underlying_points |
576+
ranges::views::transform([&](const auto & p) { return p.y; }) |
577+
ranges::to<std::vector>();
578+
ax.scatter(
579+
Args(points_x, points_y),
580+
Kwargs("color"_a = "red", "marker"_a = "o", "label"_a = "underlying"));
581+
582+
ax.legend();
583+
ax.grid();
584+
ax.set_aspect(Args("equal"));
585+
ax1.legend();
586+
ax1.grid();
587+
plt.show();
588+
return 0;
589+
}
590+
524591
int main()
525592
{
526593
pybind11::scoped_interpreter guard{};
@@ -534,6 +601,7 @@ int main()
534601
main_stairstep();
535602
*/
536603
// main_coordinate_approximation();
537-
main_curvature();
604+
// main_curvature();
605+
main_trajectory_overview();
538606
}
539607
// NOLINTEND

0 commit comments

Comments
 (0)