Skip to content

Commit 96acd2c

Browse files
soblinPanConChicharron
soblin
authored and
PanConChicharron
committed
feat(trajectory): add pretty_build() function for Planning/Control component node (autowarefoundation#332)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent c6ae942 commit 96acd2c

File tree

8 files changed

+1579
-13
lines changed

8 files changed

+1579
-13
lines changed

‎common/autoware_trajectory/README.md

+25-13
Original file line numberDiff line numberDiff line change
@@ -19,9 +19,9 @@ The interpolator class interpolates given `bases` and `values`. Following interp
1919

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

22-
```cpp title="./examples/example_readme.cpp:48:67"
22+
```cpp title="./examples/example_readme.cpp:53:68"
2323
--8<--
24-
common/autoware_trajectory/examples/example_readme.cpp:48:67
24+
common/autoware_trajectory/examples/example_readme.cpp:53:68
2525
--8<--
2626
```
2727

@@ -168,13 +168,15 @@ Each derived class in the diagram inherits the methods of all of its descending
168168

169169
### Utility functions
170170

171-
| Header / function | description | detail |
172-
| ------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
173-
| `<autoware/trajectory/utils/shift.hpp` | | ![shift](./images/utils/shift/shift.drawio.svg)<br>[View in Drawio]({{ drawio("/common/autoware_trajectory/images/utils/shift/shift.drawio.svg") }})<br>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. |
174-
| <ul><li>`struct ShiftedTrajectory`</li></ul> | contains<br><ul><li>`trajectory: Trajectory`</li><li>`shift_start_s: double`</li><li>`shift_end_s: double`</li></ul> | Returns the shifted trajectory as well the `s` values indicating where the shift starts and completes on the new shifted trajectory.<br>![shift_interval](./images/utils/shift/shift_interval.drawio.svg)[View in Drawio]({{ drawio("/common/autoware_trajectory/images/utils/shift/shift_interval.drawio.svg") }}) |
175-
| <ul><li>`struct ShiftInterval`</li></ul> | contains<br><ul><li>`start: double`</li><li>`end: double`</li><li>`lateral_offset: double`</li></ul> | <ul><li>`start < end` is required.</li><li>If `lateral_offset` is positive, the path is shifted on the right side, otherwise on the left side.</li></ul><br>![shift_left_right](./images/utils/shift/shift_left_right.drawio.svg)[View in Drawio]({{ drawio("/common/autoware_trajectory/images/utils/shift/shift_left_right.drawio.svg") }}) |
176-
| <ul><li>`struct ShiftParameters`</li></ul> | contains<br><ul><li>`velocity: double`</li><li>`lateral_acc_limit: double`</li><li>`longitudinal_acc: double`</li></ul> | <ul><li>`velocity` needs to be positive.</li></ul> |
177-
| <ul><li>`shift(const &Trajectory, const &ShiftInterval, const &ShiftParameters) -> expected<ShiftedTrajectory, ShiftError>`</li></ul> | Following [formulation](#derivation-of-shift), 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](#derivation-of-shift).<br>The example code for this plot is found [example](#shift-trajectory) |
171+
| Header / function | description | detail |
172+
| ------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
173+
| `<autoware/trajectory/utils/shift.hpp` | | ![shift](./images/utils/shift/shift.drawio.svg)<br>[View in Drawio]({{ drawio("/common/autoware_trajectory/images/utils/shift/shift.drawio.svg") }})<br>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. |
174+
| <ul><li>`struct ShiftedTrajectory`</li></ul> | contains<br><ul><li>`trajectory: Trajectory`</li><li>`shift_start_s: double`</li><li>`shift_end_s: double`</li></ul> | Returns the shifted trajectory as well the `s` values indicating where the shift starts and completes on the new shifted trajectory.<br>![shift_interval](./images/utils/shift/shift_interval.drawio.svg)[View in Drawio]({{ drawio("/common/autoware_trajectory/images/utils/shift/shift_interval.drawio.svg") }}) |
175+
| <ul><li>`struct ShiftInterval`</li></ul> | contains<br><ul><li>`start: double`</li><li>`end: double`</li><li>`lateral_offset: double`</li></ul> | <ul><li>`start < end` is required.</li><li>If `lateral_offset` is positive, the path is shifted on the right side, otherwise on the left side.</li></ul><br>![shift_left_right](./images/utils/shift/shift_left_right.drawio.svg)[View in Drawio]({{ drawio("/common/autoware_trajectory/images/utils/shift/shift_left_right.drawio.svg") }}) |
176+
| <ul><li>`struct ShiftParameters`</li></ul> | contains<br><ul><li>`velocity: double`</li><li>`lateral_acc_limit: double`</li><li>`longitudinal_acc: double`</li></ul> | <ul><li>`velocity` needs to be positive.</li></ul> |
177+
| <ul><li>`shift(const &Trajectory, const &ShiftInterval, const &ShiftParameters) -> expected<ShiftedTrajectory, ShiftError>`</li></ul> | Following [formulation](#derivation-of-shift), 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](#derivation-of-shift).<br>The example code for this plot is found [example](#shift-trajectory) |
178+
| `<autoware/trajectory/utils/pretty_build.hpp>` | | |
179+
| <ul><li>`pretty_build`</li></ul> | A convenient function that will **almost surely succeed** in constructing a Trajectory class unless the given points size is 0 or 1.<br>Input points are interpolated to 3 points using `Linear` and to 4 points using `Cubic` so that it returns<br><ul><li>`Cubic` interpolated Trajectory class(by default), or</li><li>`Akima` interpolated class if the parameter `use_akima = true`</li></ul>All of the properties are interpolated by `default` interpolators setting.<br>You may need to call `align_orientation_with_trajectory_direction` if you did not give desired orientation. | ![pretty_trajectory](./images/utils/pretty_trajectory.drawio.svg)[View in Drawio]({{ drawio("/common/autoware_trajectory/images/utils/pretty_trajectory.drawio.svg") }}) |
178180

179181
#### Derivation of `shift`
180182

@@ -238,6 +240,16 @@ $$
238240

239241
## Example Usage
240242

243+
### `pretty_build`
244+
245+
`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.
246+
247+
```cpp title="./examples/example_pretty_build.cpp:93:97"
248+
--8<--
249+
common/autoware_trajectory/examples/example_pretty_build.cpp:93:97
250+
--8<--
251+
```
252+
241253
### use custom Interpolator
242254

243255
You can also specify interpolation method to `Builder{}` before calling `.build(points)`
@@ -275,8 +287,8 @@ Set 3.0[m] ~ 5.0[m] part of velocity to 0.0
275287
trajectory->longitudinal_velocity_mps(3.0, 5.0) = 0.0;
276288
```
277289
278-
- Restore points
290+
### Restore points
279291
280-
```cpp
281-
std::vector<autoware_planning_msgs::msg::PathPoint> points = trajectory->restore();
282-
```
292+
```cpp
293+
std::vector<autoware_planning_msgs::msg::PathPoint> points = trajectory->restore();
294+
```

0 commit comments

Comments
 (0)