14
14
15
15
#include " autoware/interpolation/spline_interpolation.hpp"
16
16
#include " autoware/interpolation/spline_interpolation_points_2d.hpp"
17
- #include " autoware/universe_utils /geometry/geometry.hpp"
17
+ #include " autoware_utils /geometry/geometry.hpp"
18
18
19
19
#include < gtest/gtest.h>
20
20
@@ -27,15 +27,15 @@ using autoware::interpolation::SplineInterpolationPoints2d;
27
27
28
28
TEST (spline_interpolation, splineYawFromPoints)
29
29
{
30
- using autoware::universe_utils::createPoint ;
30
+ using autoware_utils::create_point ;
31
31
32
32
{ // straight
33
33
std::vector<geometry_msgs::msg::Point > points;
34
- points.push_back (createPoint (0.0 , 0.0 , 0.0 ));
35
- points.push_back (createPoint (1.0 , 1.5 , 0.0 ));
36
- points.push_back (createPoint (2.0 , 3.0 , 0.0 ));
37
- points.push_back (createPoint (3.0 , 4.5 , 0.0 ));
38
- points.push_back (createPoint (4.0 , 6.0 , 0.0 ));
34
+ points.push_back (create_point (0.0 , 0.0 , 0.0 ));
35
+ points.push_back (create_point (1.0 , 1.5 , 0.0 ));
36
+ points.push_back (create_point (2.0 , 3.0 , 0.0 ));
37
+ points.push_back (create_point (3.0 , 4.5 , 0.0 ));
38
+ points.push_back (create_point (4.0 , 6.0 , 0.0 ));
39
39
40
40
const std::vector<double > ans{0.9827937 , 0.9827937 , 0.9827937 , 0.9827937 , 0.9827937 };
41
41
@@ -47,11 +47,11 @@ TEST(spline_interpolation, splineYawFromPoints)
47
47
48
48
{ // curve
49
49
std::vector<geometry_msgs::msg::Point > points;
50
- points.push_back (createPoint (-2.0 , -10.0 , 0.0 ));
51
- points.push_back (createPoint (2.0 , 1.5 , 0.0 ));
52
- points.push_back (createPoint (3.0 , 3.0 , 0.0 ));
53
- points.push_back (createPoint (5.0 , 10.0 , 0.0 ));
54
- points.push_back (createPoint (10.0 , 12.5 , 0.0 ));
50
+ points.push_back (create_point (-2.0 , -10.0 , 0.0 ));
51
+ points.push_back (create_point (2.0 , 1.5 , 0.0 ));
52
+ points.push_back (create_point (3.0 , 3.0 , 0.0 ));
53
+ points.push_back (create_point (5.0 , 10.0 , 0.0 ));
54
+ points.push_back (create_point (10.0 , 12.5 , 0.0 ));
55
55
56
56
const std::vector<double > ans{1.368174 , 0.961318 , 1.086098 , 0.938357 , 0.278594 };
57
57
const auto yaws = autoware::interpolation::splineYawFromPoints (points);
@@ -62,15 +62,15 @@ TEST(spline_interpolation, splineYawFromPoints)
62
62
63
63
{ // size of base_keys is 1 (infeasible to interpolate)
64
64
std::vector<geometry_msgs::msg::Point > points;
65
- points.push_back (createPoint (1.0 , 0.0 , 0.0 ));
65
+ points.push_back (create_point (1.0 , 0.0 , 0.0 ));
66
66
67
67
EXPECT_THROW (autoware::interpolation::splineYawFromPoints (points), std::logic_error);
68
68
}
69
69
70
70
{ // straight: size of base_keys is 2 (edge case in the implementation)
71
71
std::vector<geometry_msgs::msg::Point > points;
72
- points.push_back (createPoint (1.0 , 0.0 , 0.0 ));
73
- points.push_back (createPoint (2.0 , 1.5 , 0.0 ));
72
+ points.push_back (create_point (1.0 , 0.0 , 0.0 ));
73
+ points.push_back (create_point (2.0 , 1.5 , 0.0 ));
74
74
75
75
const std::vector<double > ans{0.9827937 , 0.9827937 };
76
76
@@ -82,9 +82,9 @@ TEST(spline_interpolation, splineYawFromPoints)
82
82
83
83
{ // straight: size of base_keys is 3 (edge case in the implementation)
84
84
std::vector<geometry_msgs::msg::Point > points;
85
- points.push_back (createPoint (1.0 , 0.0 , 0.0 ));
86
- points.push_back (createPoint (2.0 , 1.5 , 0.0 ));
87
- points.push_back (createPoint (3.0 , 3.0 , 0.0 ));
85
+ points.push_back (create_point (1.0 , 0.0 , 0.0 ));
86
+ points.push_back (create_point (2.0 , 1.5 , 0.0 ));
87
+ points.push_back (create_point (3.0 , 3.0 , 0.0 ));
88
88
89
89
const std::vector<double > ans{0.9827937 , 0.9827937 , 0.9827937 };
90
90
@@ -97,15 +97,15 @@ TEST(spline_interpolation, splineYawFromPoints)
97
97
98
98
TEST (spline_interpolation, SplineInterpolationPoints2d)
99
99
{
100
- using autoware::universe_utils::createPoint ;
100
+ using autoware_utils::create_point ;
101
101
102
102
// curve
103
103
std::vector<geometry_msgs::msg::Point > points;
104
- points.push_back (createPoint (-2.0 , -10.0 , 0.0 ));
105
- points.push_back (createPoint (2.0 , 1.5 , 0.0 ));
106
- points.push_back (createPoint (3.0 , 3.0 , 0.0 ));
107
- points.push_back (createPoint (5.0 , 10.0 , 0.0 ));
108
- points.push_back (createPoint (10.0 , 12.5 , 0.0 ));
104
+ points.push_back (create_point (-2.0 , -10.0 , 0.0 ));
105
+ points.push_back (create_point (2.0 , 1.5 , 0.0 ));
106
+ points.push_back (create_point (3.0 , 3.0 , 0.0 ));
107
+ points.push_back (create_point (5.0 , 10.0 , 0.0 ));
108
+ points.push_back (create_point (10.0 , 12.5 , 0.0 ));
109
109
110
110
SplineInterpolationPoints2d s (points);
111
111
@@ -194,19 +194,19 @@ TEST(spline_interpolation, SplineInterpolationPoints2d)
194
194
195
195
// size of base_keys is 1 (infeasible to interpolate)
196
196
std::vector<geometry_msgs::msg::Point > single_points;
197
- single_points.push_back (createPoint (1.0 , 0.0 , 0.0 ));
197
+ single_points.push_back (create_point (1.0 , 0.0 , 0.0 ));
198
198
EXPECT_THROW (SplineInterpolationPoints2d{single_points}, std::logic_error);
199
199
}
200
200
201
201
TEST (spline_interpolation, SplineInterpolationPoints2dPolymorphism)
202
202
{
203
- using autoware::universe_utils::createPoint;
204
203
using autoware_planning_msgs::msg::TrajectoryPoint;
204
+ using autoware_utils::create_point;
205
205
206
206
std::vector<geometry_msgs::msg::Point > points;
207
- points.push_back (createPoint (-2.0 , -10.0 , 0.0 ));
208
- points.push_back (createPoint (2.0 , 1.5 , 0.0 ));
209
- points.push_back (createPoint (3.0 , 3.0 , 0.0 ));
207
+ points.push_back (create_point (-2.0 , -10.0 , 0.0 ));
208
+ points.push_back (create_point (2.0 , 1.5 , 0.0 ));
209
+ points.push_back (create_point (3.0 , 3.0 , 0.0 ));
210
210
211
211
std::vector<TrajectoryPoint> trajectory_points;
212
212
for (const auto & p : points) {
0 commit comments