Skip to content

feat(trajectory): add pretty_build() function for Planning/Control component node #332

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 6 commits into from
Apr 15, 2025
Merged

feat(trajectory): add pretty_build() function for Planning/Control component node #332

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
576f31f
feat(trajectory): add populate function
soblin Mar 26, 2025
ef799b5
Merge branch 'main' into feat/trajectory-add-populate-function
soblin Apr 14, 2025
b5d25ed
move namespace to experimental
soblin Apr 15, 2025
6d7e4b3
reflect comment
soblin Apr 15, 2025
8c9c68a
minor fix readme
soblin Apr 15, 2025
1cdaef5
fix
soblin Apr 15, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
38 changes: 25 additions & 13 deletions common/autoware_trajectory/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,9 @@ The interpolator class interpolates given `bases` and `values`. Following interp

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

```cpp title="./examples/example_readme.cpp:48:67"
```cpp title="./examples/example_readme.cpp:53:68"
--8<--
common/autoware_trajectory/examples/example_readme.cpp:48:67
common/autoware_trajectory/examples/example_readme.cpp:53:68
--8<--
```

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

### Utility functions

| Header / function | description | detail |
| ------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| `<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. |
| <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") }}) |
| <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") }}) |
| <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> |
| <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) |
| Header / function | description | detail |
| ------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| `<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. |
| <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") }}) |
| <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") }}) |
| <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> |
| <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) |
| `<autoware/trajectory/utils/pretty_build.hpp>` | | |
| <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") }}) |

#### Derivation of `shift`

Expand Down Expand Up @@ -238,6 +240,16 @@ $$

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

```cpp title="./examples/example_pretty_build.cpp:93:97"
--8<--
common/autoware_trajectory/examples/example_pretty_build.cpp:93:97
--8<--
```

### use custom Interpolator

You can also specify interpolation method to `Builder{}` before calling `.build(points)`
Expand Down Expand Up @@ -275,8 +287,8 @@ 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
### Restore points

```cpp
std::vector<autoware_planning_msgs::msg::PathPoint> points = trajectory->restore();
```
```cpp
std::vector<autoware_planning_msgs::msg::PathPoint> points = trajectory->restore();
```
Loading
Loading