Skip to content

Commit 536b83c

Browse files
soblinsoblin
committed
update align_orientation_with_trajectory_direction fig
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent a4b94df commit 536b83c

File tree

5 files changed

+86
-40
lines changed

5 files changed

+86
-40
lines changed

‎common/autoware_trajectory/README.md

Lines changed: 16 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -134,16 +134,22 @@ Several `Trajectory<T>` are defined in the following inheritance hierarchy accor
134134

135135
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.
136136

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 |
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 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 |
147+
| | `elevation(const double s)` | return the elevation angle at given `s` coordinate. | |
148+
| | `get_underlying_base()` | return the vector of `s` values of current `underlying` points. | |
149+
| `<autoware/trajectory/pose.hpp>`<br><ul><li>`Trajectory<geometry_msgs::msg::Pose>::Builder`</li></ul> | `Builder()` | set default interpolator setting in addition to that of `Trajectory<Point>::Builder` as follows.<br><ul><li>`orientation`: SphericalLinear</li></ul> | |
150+
| | `set_orientation_interpolator<InterpolatorType>()` | set custom interpolator for `orientation`. | |
151+
| <ul><li>`Trajectory<Pose>`</li></ul> | derives all of the above methods of `Trajectory<Point>` | | |
152+
| | `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. | ![align_orientation_with_trajectory_direction](./images/utils/align_orientation_with_trajectory_direction.drawio.svg)<br>[View in Drawio]({{ drawio("/common/autoware_trajectory/images/utils/align_orientation_with_trajectory_direction.drawio.svg") }}) |
147153

148154
## Example Usage
149155

‎common/autoware_trajectory/examples/example_pose.cpp

Lines changed: 40 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -13,8 +13,10 @@
1313
// limitations under the License.
1414

1515
#include "autoware/trajectory/pose.hpp"
16+
#include "autoware_utils_geometry/geometry.hpp"
1617

1718
#include <autoware/pyplot/pyplot.hpp>
19+
#include <range/v3/all.hpp>
1820
#include <tf2/LinearMath/Quaternion.hpp>
1921
#include <tf2/LinearMath/Vector3.hpp>
2022

@@ -23,6 +25,8 @@
2325
#include <pybind11/embed.h>
2426
#include <pybind11/stl.h>
2527

28+
#include <algorithm>
29+
#include <string>
2630
#include <vector>
2731

2832
geometry_msgs::msg::Pose pose(double x, double y)
@@ -34,11 +38,37 @@ geometry_msgs::msg::Pose pose(double x, double y)
3438
return p;
3539
}
3640

41+
using autoware::trajectory::Trajectory;
42+
using ranges::to;
43+
using ranges::views::transform;
44+
45+
void plot_trajectory_base_with_orientation(
46+
const Trajectory<geometry_msgs::msg::Pose> & trajectory, const std::string & label,
47+
autoware::pyplot::Axes & ax)
48+
{
49+
const auto s = trajectory.get_underlying_bases();
50+
const auto c = trajectory.compute(s);
51+
const auto x =
52+
c | transform([](const auto & point) { return point.position.x; }) | to<std::vector>();
53+
const auto y =
54+
c | transform([](const auto & point) { return point.position.y; }) | to<std::vector>();
55+
const auto th =
56+
c | transform([](const auto & quat) { return autoware_utils_geometry::get_rpy(quat).z; }) |
57+
to<std::vector>();
58+
const auto cos_th = th | transform([](const auto v) { return std::cos(v); }) | to<std::vector>();
59+
const auto sin_th = th | transform([](const auto v) { return std::sin(v); }) | to<std::vector>();
60+
ax.scatter(Args(x, y), Kwargs("color"_a = "red", "marker"_a = "o", "label"_a = "underlying"));
61+
ax.quiver(Args(x, y, cos_th, sin_th), Kwargs("color"_a = "green", "label"_a = label));
62+
}
63+
3764
int main()
3865
{
3966
pybind11::scoped_interpreter guard{};
4067

4168
auto plt = autoware::pyplot::import();
69+
auto [fig, axes] = plt.subplots(1, 2);
70+
auto & ax1 = axes[0];
71+
auto & ax2 = axes[1];
4272

4373
std::vector<geometry_msgs::msg::Pose> poses = {
4474
pose(0.49, 0.59), pose(0.61, 1.22), pose(0.86, 1.93), pose(1.20, 2.56), pose(1.51, 3.17),
@@ -47,11 +77,12 @@ int main()
4777
pose(6.88, 3.54), pose(7.51, 4.25), pose(7.85, 4.93), pose(8.03, 5.73), pose(8.16, 6.52),
4878
pose(8.31, 7.28), pose(8.45, 7.93), pose(8.68, 8.45), pose(8.96, 8.96), pose(9.32, 9.36)};
4979

50-
using autoware::trajectory::Trajectory;
51-
5280
auto trajectory = Trajectory<geometry_msgs::msg::Pose>::Builder{}.build(poses);
5381

82+
plot_trajectory_base_with_orientation(*trajectory, "before", ax1);
5483
trajectory->align_orientation_with_trajectory_direction();
84+
plot_trajectory_base_with_orientation(
85+
*trajectory, "after align_orientation_with_trajectory_direction()", ax2);
5586

5687
{
5788
std::vector<double> x;
@@ -63,34 +94,15 @@ int main()
6394
y.push_back(p.position.y);
6495
}
6596

66-
plt.plot(Args(x, y), Kwargs("label"_a = "Trajectory", "color"_a = "blue"));
97+
ax1.plot(Args(x, y), Kwargs("label"_a = "Trajectory", "color"_a = "blue"));
98+
ax2.plot(Args(x, y), Kwargs("label"_a = "Trajectory", "color"_a = "blue"));
6799
}
68100

69-
{
70-
std::vector<double> x;
71-
std::vector<double> y;
72-
std::vector<double> dx;
73-
std::vector<double> dy;
74-
75-
for (double i = 0.0; i <= trajectory->length(); i += 1.0) {
76-
auto p = trajectory->compute(i);
77-
x.push_back(p.position.x);
78-
y.push_back(p.position.y);
79-
80-
tf2::Vector3 x_axis(1.0, 0.0, 0.0);
81-
tf2::Quaternion q(p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
82-
tf2::Vector3 direction = tf2::quatRotate(q, x_axis);
83-
dx.push_back(direction.x());
84-
dy.push_back(direction.y());
85-
// double azimuth = trajectory->azimuth(i);
86-
// dx.push_back(std::cos(azimuth));
87-
// dy.push_back(std::sin(azimuth));
88-
}
89-
plt.quiver(Args(x, y, dx, dy), Kwargs("label"_a = "Direction", "color"_a = "green"));
101+
fig.tight_layout();
102+
for (auto & ax : axes) {
103+
ax.set_aspect(Args("equal"));
104+
ax.grid();
105+
ax.legend();
90106
}
91-
92-
plt.axis(Args("equal"));
93-
plt.grid();
94-
plt.legend();
95107
plt.show();
96108
}

‎common/autoware_trajectory/examples/example_readme.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -513,7 +513,7 @@ int main_curvature()
513513

514514
ax1.plot(Args(s, cubic.curvature(s)), Kwargs("color"_a = "navy", "label"_a = "curvature"));
515515
ax.quiver(
516-
Args(points_x, points_y, cos_yaw, sin_yaw), Kwargs("color"_a = "green", "label"_a = "yaw"));
516+
Args(points_x, points_y, cos_yaw, sin_yaw), Kwargs("color"_a = "green", "label"_a = "azimuth"));
517517

518518
fig.tight_layout();
519519
for (auto & a : axes) {

‎common/autoware_trajectory/images/utils/align_orientation_with_trajectory_direction.drawio.svg

Lines changed: 28 additions & 0 deletions
Loading

‎common/autoware_trajectory/src/pose.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -94,7 +94,7 @@ PointType Trajectory<PointType>::compute(const double s) const
9494
PointType result;
9595
result.position = BaseClass::compute(s);
9696
const auto s_clamp = clamp(s);
97-
// NOTE(soblin): Ideally azimuth() should be used? But okay if the interpolation is not rough
97+
// NOTE(soblin): azimuth() should be not used here to serve as interpolator
9898
result.orientation = orientation_interpolator_->compute(s_clamp);
9999
return result;
100100
}

0 commit comments

Comments
 (0)