Skip to content

Commit 7caa86c

Browse files
yhisakiSakodaShintaro
yhisaki
authored and
SakodaShintaro
committed
feat(autoware_trajectory): update autoware_trajectory interfaces (autowarefoundation#10034)
* feat(autoware_trajectory): update autoware_trajectory interfaces Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * add const Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * Update shift.hpp Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * fix names Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> * fix copyright Signed-off-by: Y.Hisaki <yhisaki31@gmail.com> --------- Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>
1 parent 3f791c5 commit 7caa86c

38 files changed

+1951
-442
lines changed

‎common/autoware_trajectory/examples/example_find_intervals.cpp

+108
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,108 @@
1+
// Copyright 2025 Tier IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "autoware/trajectory/path_point_with_lane_id.hpp"
16+
#include "autoware/trajectory/utils/find_intervals.hpp"
17+
18+
#include <matplotlibcpp17/pyplot.h>
19+
20+
#include <vector>
21+
22+
using Trajectory = autoware::trajectory::Trajectory<tier4_planning_msgs::msg::PathPointWithLaneId>;
23+
24+
tier4_planning_msgs::msg::PathPointWithLaneId path_point_with_lane_id(
25+
double x, double y, uint8_t lane_id)
26+
{
27+
tier4_planning_msgs::msg::PathPointWithLaneId point;
28+
point.point.pose.position.x = x;
29+
point.point.pose.position.y = y;
30+
point.lane_ids.emplace_back(lane_id);
31+
return point;
32+
}
33+
34+
int main()
35+
{
36+
pybind11::scoped_interpreter guard{};
37+
auto plt = matplotlibcpp17::pyplot::import();
38+
39+
std::vector<tier4_planning_msgs::msg::PathPointWithLaneId> points{
40+
path_point_with_lane_id(0.41, 0.69, 0), path_point_with_lane_id(0.66, 1.09, 0),
41+
path_point_with_lane_id(0.93, 1.41, 0), path_point_with_lane_id(1.26, 1.71, 0),
42+
path_point_with_lane_id(1.62, 1.90, 0), path_point_with_lane_id(1.96, 1.98, 0),
43+
path_point_with_lane_id(2.48, 1.96, 1), path_point_with_lane_id(3.02, 1.87, 1),
44+
path_point_with_lane_id(3.56, 1.82, 1), path_point_with_lane_id(4.14, 2.02, 1),
45+
path_point_with_lane_id(4.56, 2.36, 1), path_point_with_lane_id(4.89, 2.72, 1),
46+
path_point_with_lane_id(5.27, 3.15, 1), path_point_with_lane_id(5.71, 3.69, 1),
47+
path_point_with_lane_id(6.09, 4.02, 0), path_point_with_lane_id(6.54, 4.16, 0),
48+
path_point_with_lane_id(6.79, 3.92, 0), path_point_with_lane_id(7.11, 3.60, 0),
49+
path_point_with_lane_id(7.42, 3.01, 0)};
50+
51+
auto trajectory = Trajectory::Builder{}.build(points);
52+
53+
if (!trajectory) {
54+
return 1;
55+
}
56+
57+
const auto intervals = autoware::trajectory::find_intervals(
58+
*trajectory, [](const tier4_planning_msgs::msg::PathPointWithLaneId & point) {
59+
return point.lane_ids[0] == 1;
60+
});
61+
62+
std::vector<double> x_id0;
63+
std::vector<double> y_id0;
64+
std::vector<double> x_id1;
65+
std::vector<double> y_id1;
66+
std::vector<double> x_all;
67+
std::vector<double> y_all;
68+
69+
for (const auto & point : points) {
70+
if (point.lane_ids[0] == 0) {
71+
x_id0.push_back(point.point.pose.position.x);
72+
y_id0.push_back(point.point.pose.position.y);
73+
} else {
74+
x_id1.push_back(point.point.pose.position.x);
75+
y_id1.push_back(point.point.pose.position.y);
76+
}
77+
}
78+
79+
for (double s = 0.0; s < trajectory->length(); s += 0.01) {
80+
auto p = trajectory->compute(s);
81+
x_all.push_back(p.point.pose.position.x);
82+
y_all.push_back(p.point.pose.position.y);
83+
}
84+
85+
plt.plot(Args(x_all, y_all), Kwargs("color"_a = "blue"));
86+
plt.scatter(Args(x_id0, y_id0), Kwargs("color"_a = "blue", "label"_a = "lane_id = 0"));
87+
plt.scatter(Args(x_id1, y_id1), Kwargs("color"_a = "red", "label"_a = "lane_id = 1"));
88+
89+
std::vector<double> x_cropped;
90+
std::vector<double> y_cropped;
91+
92+
trajectory->crop(intervals[0].start, intervals[0].end - intervals[0].start);
93+
94+
for (double s = 0.0; s < trajectory->length(); s += 0.01) {
95+
auto p = trajectory->compute(s);
96+
x_cropped.push_back(p.point.pose.position.x);
97+
y_cropped.push_back(p.point.pose.position.y);
98+
}
99+
100+
plt.plot(
101+
Args(x_cropped, y_cropped),
102+
Kwargs("color"_a = "red", "label"_a = "interval between lane_id = 1"));
103+
104+
plt.legend();
105+
plt.show();
106+
107+
return 0;
108+
}

‎common/autoware_trajectory/examples/example_shift.cpp

+187
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,187 @@
1+
// Copyright 2025 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "autoware/trajectory/point.hpp"
16+
#include "autoware/trajectory/utils/shift.hpp"
17+
18+
#include <matplotlibcpp17/pyplot.h>
19+
20+
#include <iostream>
21+
#include <vector>
22+
23+
geometry_msgs::msg::Point point(double x, double y)
24+
{
25+
geometry_msgs::msg::Point p;
26+
p.x = x;
27+
p.y = y;
28+
return p;
29+
}
30+
31+
int main()
32+
{
33+
pybind11::scoped_interpreter guard{};
34+
auto plt = matplotlibcpp17::pyplot::import();
35+
36+
geometry_msgs::msg::Point p;
37+
(void)(p);
38+
39+
std::vector<geometry_msgs::msg::Point> points = {
40+
point(0.49, 0.59), point(0.61, 1.22), point(0.86, 1.93), point(1.20, 2.56), point(1.51, 3.17),
41+
point(1.85, 3.76), point(2.14, 4.26), point(2.60, 4.56), point(3.07, 4.55), point(3.61, 4.30),
42+
point(3.95, 4.01), point(4.29, 3.68), point(4.90, 3.25), point(5.54, 3.10), point(6.24, 3.18),
43+
point(6.88, 3.54), point(7.51, 4.25), point(7.85, 4.93), point(8.03, 5.73), point(8.16, 6.52),
44+
point(8.31, 7.28), point(8.45, 7.93), point(8.68, 8.45), point(8.96, 8.96), point(9.32, 9.36)};
45+
46+
auto trajectory =
47+
autoware::trajectory::Trajectory<geometry_msgs::msg::Point>::Builder{}.build(points);
48+
49+
if (!trajectory) {
50+
return 1;
51+
}
52+
53+
std::cout << "length: " << trajectory->length() << std::endl;
54+
55+
{
56+
std::vector<double> x;
57+
std::vector<double> y;
58+
for (double s = 0.0; s < trajectory->length(); s += 0.01) {
59+
auto p = trajectory->compute(s);
60+
x.push_back(p.x);
61+
y.push_back(p.y);
62+
}
63+
plt.plot(Args(x, y), Kwargs("label"_a = "original"));
64+
65+
x.clear();
66+
y.clear();
67+
68+
autoware::trajectory::ShiftInterval shift_interval;
69+
shift_interval.end = -1.0;
70+
shift_interval.lateral_offset = 0.5;
71+
72+
auto shifted_trajectory = autoware::trajectory::shift(*trajectory, shift_interval);
73+
74+
for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) {
75+
auto p = shifted_trajectory.compute(s);
76+
x.push_back(p.x);
77+
y.push_back(p.y);
78+
}
79+
80+
plt.axis(Args("equal"));
81+
plt.plot(Args(x, y), Kwargs("label"_a = "shifted"));
82+
plt.legend();
83+
plt.show();
84+
}
85+
86+
{
87+
std::vector<double> x;
88+
std::vector<double> y;
89+
for (double s = 0.0; s < trajectory->length(); s += 0.01) {
90+
auto p = trajectory->compute(s);
91+
x.push_back(p.x);
92+
y.push_back(p.y);
93+
}
94+
plt.plot(Args(x, y), Kwargs("label"_a = "original"));
95+
96+
x.clear();
97+
y.clear();
98+
99+
autoware::trajectory::ShiftInterval shift_interval;
100+
shift_interval.start = trajectory->length() / 4.0;
101+
shift_interval.end = trajectory->length() * 3.0 / 4.0;
102+
shift_interval.lateral_offset = 0.5;
103+
auto shifted_trajectory = autoware::trajectory::shift(*trajectory, shift_interval);
104+
105+
for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) {
106+
auto p = shifted_trajectory.compute(s);
107+
x.push_back(p.x);
108+
y.push_back(p.y);
109+
}
110+
111+
plt.axis(Args("equal"));
112+
plt.plot(Args(x, y), Kwargs("label"_a = "shifted"));
113+
plt.legend();
114+
plt.show();
115+
}
116+
117+
{
118+
std::vector<double> x;
119+
std::vector<double> y;
120+
for (double s = 0.0; s < trajectory->length(); s += 0.01) {
121+
auto p = trajectory->compute(s);
122+
x.push_back(p.x);
123+
y.push_back(p.y);
124+
}
125+
plt.plot(Args(x, y), Kwargs("label"_a = "original"));
126+
127+
x.clear();
128+
y.clear();
129+
130+
autoware::trajectory::ShiftInterval shift_interval;
131+
shift_interval.start = trajectory->length() * 3.0 / 4.0;
132+
shift_interval.end = trajectory->length() / 4.0;
133+
shift_interval.lateral_offset = 0.5;
134+
auto shifted_trajectory = autoware::trajectory::shift(*trajectory, shift_interval);
135+
136+
for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) {
137+
auto p = shifted_trajectory.compute(s);
138+
x.push_back(p.x);
139+
y.push_back(p.y);
140+
}
141+
142+
plt.axis(Args("equal"));
143+
plt.plot(Args(x, y), Kwargs("label"_a = "shifted"));
144+
plt.legend();
145+
plt.show();
146+
}
147+
148+
{
149+
std::vector<double> x;
150+
std::vector<double> y;
151+
for (double s = 0.0; s < trajectory->length(); s += 0.01) {
152+
auto p = trajectory->compute(s);
153+
x.push_back(p.x);
154+
y.push_back(p.y);
155+
}
156+
plt.plot(Args(x, y), Kwargs("label"_a = "original"));
157+
158+
x.clear();
159+
y.clear();
160+
161+
autoware::trajectory::ShiftInterval shift_interval1;
162+
shift_interval1.start = trajectory->length() / 4.0;
163+
shift_interval1.end = trajectory->length() * 2.0 / 4.0;
164+
shift_interval1.lateral_offset = 0.5;
165+
166+
autoware::trajectory::ShiftInterval shift_interval2;
167+
shift_interval2.start = trajectory->length() * 2.0 / 4.0;
168+
shift_interval2.end = trajectory->length() * 3.0 / 4.0;
169+
shift_interval2.lateral_offset = -0.5;
170+
171+
auto shifted_trajectory =
172+
autoware::trajectory::shift(*trajectory, {shift_interval1, shift_interval2});
173+
174+
for (double s = 0.0; s < shifted_trajectory.length(); s += 0.01) {
175+
auto p = shifted_trajectory.compute(s);
176+
x.push_back(p.x);
177+
y.push_back(p.y);
178+
}
179+
180+
plt.axis(Args("equal"));
181+
plt.plot(Args(x, y), Kwargs("label"_a = "shifted"));
182+
plt.legend();
183+
plt.show();
184+
}
185+
186+
return 0;
187+
}

‎common/autoware_trajectory/examples/example_trajectory_path_point.cpp

+14-10
Original file line numberDiff line numberDiff line change
@@ -13,10 +13,15 @@
1313
// limitations under the License.
1414

1515
#include "autoware/trajectory/path_point.hpp"
16+
#include "autoware/trajectory/utils/crossed.hpp"
17+
#include "lanelet2_core/primitives/LineString.h"
1618

1719
#include <autoware_planning_msgs/msg/path_point.hpp>
1820
#include <geometry_msgs/msg/point.hpp>
1921

22+
#include <boost/geometry/geometries/linestring.hpp>
23+
24+
#include <Eigen/src/Core/Matrix.h>
2025
#include <matplotlibcpp17/pyplot.h>
2126

2227
#include <iostream>
@@ -69,25 +74,24 @@ int main()
6974

7075
trajectory->align_orientation_with_trajectory_direction();
7176

72-
geometry_msgs::msg::Point p1;
73-
geometry_msgs::msg::Point p2;
74-
p1.x = 7.5;
75-
p1.y = 8.6;
76-
p2.x = 10.2;
77-
p2.y = 7.7;
77+
lanelet::LineString2d line_string;
78+
line_string.push_back(lanelet::Point3d(lanelet::InvalId, 7.5, 8.6, 0.0));
79+
line_string.push_back(lanelet::Point3d(lanelet::InvalId, 10.2, 7.7, 0.0));
7880

79-
auto s = trajectory->crossed(p1, p2);
80-
auto crossed = trajectory->compute(s.value());
81+
auto s = autoware::trajectory::crossed(*trajectory, line_string);
82+
auto crossed = trajectory->compute(s.at(0));
8183

8284
plt.plot(
83-
Args(std::vector<double>{p1.x, p2.x}, std::vector<double>{p1.y, p2.y}),
85+
Args(
86+
std::vector<double>{line_string[0].x(), line_string[1].x()},
87+
std::vector<double>{line_string[0].y(), line_string[1].y()}),
8488
Kwargs("color"_a = "purple"));
8589

8690
plt.scatter(
8791
Args(crossed.pose.position.x, crossed.pose.position.y),
8892
Kwargs("label"_a = "Crossed on trajectory", "color"_a = "purple"));
8993

90-
trajectory->longitudinal_velocity_mps.range(s.value(), trajectory->length()).set(0.0);
94+
trajectory->longitudinal_velocity_mps().range(s.at(0), trajectory->length()).set(0.0);
9195

9296
std::vector<double> x;
9397
std::vector<double> y;

‎common/autoware_trajectory/examples/example_trajectory_point.cpp

+18-11
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,9 @@
1414

1515
#include "autoware/trajectory/interpolator/cubic_spline.hpp"
1616
#include "autoware/trajectory/point.hpp"
17+
#include "autoware/trajectory/utils/closest.hpp"
18+
#include "autoware/trajectory/utils/crossed.hpp"
19+
#include "lanelet2_core/primitives/LineString.h"
1720

1821
#include <geometry_msgs/msg/point.hpp>
1922

@@ -86,7 +89,8 @@ int main()
8689
p.x = 5.37;
8790
p.y = 6.0;
8891

89-
auto s = trajectory->closest(p);
92+
double s = autoware::trajectory::closest(*trajectory, p);
93+
9094
auto closest = trajectory->compute(s);
9195

9296
plt.scatter(Args(p.x, p.y), Kwargs("color"_a = "green"));
@@ -98,18 +102,21 @@ int main()
98102
Kwargs("color"_a = "green"));
99103
}
100104
{
101-
geometry_msgs::msg::Point p1;
102-
geometry_msgs::msg::Point p2;
103-
p1.x = 6.97;
104-
p1.y = 6.36;
105-
p2.x = 9.23;
106-
p2.y = 5.92;
107-
108-
auto s = trajectory->crossed(p1, p2);
109-
auto crossed = trajectory->compute(s.value());
105+
lanelet::LineString2d line_string;
106+
line_string.push_back(lanelet::Point3d(lanelet::InvalId, 6.97, 6.36, 0.0));
107+
line_string.push_back(lanelet::Point3d(lanelet::InvalId, 9.23, 5.92, 0.0));
108+
109+
auto s = autoware::trajectory::crossed(*trajectory, line_string);
110+
if (s.empty()) {
111+
std::cerr << "Failed to find a crossing point" << std::endl;
112+
return 1;
113+
}
114+
auto crossed = trajectory->compute(s.at(0));
110115

111116
plt.plot(
112-
Args(std::vector<double>{p1.x, p2.x}, std::vector<double>{p1.y, p2.y}),
117+
Args(
118+
std::vector<double>{line_string[0].x(), line_string[1].x()},
119+
std::vector<double>{line_string[0].y(), line_string[1].y()}),
113120
Kwargs("color"_a = "purple"));
114121

115122
plt.scatter(

0 commit comments

Comments
 (0)