Skip to content

Commit bb825f0

Browse files
go-sakayorigo-sakayori
authored
test(motion_utils): add test for path shift (#9083)
* remove unused function Signed-off-by: Go Sakayori <gsakayori@gmail.com> * mover path shifter utils function to autoware motion utils Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * minor change in license header Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * fix warning message Signed-off-by: Go Sakayori <gsakayori@gmail.com> * remove header file Signed-off-by: Go Sakayori <gsakayori@gmail.com> * add test file Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * add unit test to all function Signed-off-by: Go Sakayori <gsakayori@gmail.com> * fix spelling Signed-off-by: Go Sakayori <gsakayori@gmail.com> --------- Signed-off-by: Go Sakayori <gsakayori@gmail.com> Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>
1 parent 8c63c61 commit bb825f0

File tree

1 file changed

+163
-0
lines changed

1 file changed

+163
-0
lines changed

‎common/autoware_motion_utils/test/src/trajectory/test_path_shift.cpp

+163
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,163 @@
1+
// Copyright 2024 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/motion_utils/trajectory/path_shift.hpp"
16+
17+
#include <gtest/gtest.h>
18+
19+
TEST(path_shift_test, calc_feasible_velocity_from_jerk)
20+
{
21+
using autoware::motion_utils::calc_feasible_velocity_from_jerk;
22+
23+
double longitudinal_distance = 0.0;
24+
double lateral_distance = 0.0;
25+
double lateral_jerk = 0.0;
26+
27+
// Condition: zero lateral jerk
28+
EXPECT_DOUBLE_EQ(
29+
calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance),
30+
1.0e10);
31+
32+
// Condition: zero lateral distance
33+
lateral_jerk = 1.0;
34+
EXPECT_DOUBLE_EQ(
35+
calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance),
36+
1.0e10);
37+
38+
// Condition: zero longitudinal distance
39+
lateral_distance = 2.0;
40+
EXPECT_DOUBLE_EQ(
41+
calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), 0.0);
42+
43+
// Condition: random condition
44+
longitudinal_distance = 50.0;
45+
EXPECT_DOUBLE_EQ(
46+
calc_feasible_velocity_from_jerk(lateral_distance, lateral_jerk, longitudinal_distance), 12.5);
47+
}
48+
49+
TEST(path_shift_test, calc_lateral_dist_from_jerk)
50+
{
51+
using autoware::motion_utils::calc_lateral_dist_from_jerk;
52+
53+
double longitudinal_distance = 0.0;
54+
double lateral_jerk = 0.0;
55+
double velocity = 0.0;
56+
57+
// Condition: zero lateral jerk
58+
EXPECT_DOUBLE_EQ(
59+
calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 1.0e10);
60+
61+
// Condition: zero velocity
62+
lateral_jerk = 2.0;
63+
EXPECT_DOUBLE_EQ(
64+
calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 1.0e10);
65+
66+
// Condition: zero longitudinal distance
67+
velocity = 10.0;
68+
EXPECT_DOUBLE_EQ(calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 0.0);
69+
70+
// Condition: random condition
71+
longitudinal_distance = 100.0;
72+
EXPECT_DOUBLE_EQ(
73+
calc_lateral_dist_from_jerk(longitudinal_distance, lateral_jerk, velocity), 62.5);
74+
}
75+
76+
TEST(path_shift_test, calc_longitudinal_dist_from_jerk)
77+
{
78+
using autoware::motion_utils::calc_longitudinal_dist_from_jerk;
79+
80+
double lateral_distance = 0.0;
81+
double lateral_jerk = 0.0;
82+
double velocity = 0.0;
83+
84+
// Condition: zero lateral jerk
85+
EXPECT_DOUBLE_EQ(
86+
calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 1.0e10);
87+
88+
// Condition: zero lateral distance
89+
lateral_jerk = -1.0;
90+
velocity = 10.0;
91+
EXPECT_DOUBLE_EQ(calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 0.0);
92+
93+
// Condition: zero velocity
94+
velocity = 0.0;
95+
lateral_distance = 54.0;
96+
EXPECT_DOUBLE_EQ(calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 0.0);
97+
98+
// Condition: random
99+
velocity = 8.0;
100+
EXPECT_DOUBLE_EQ(
101+
calc_longitudinal_dist_from_jerk(lateral_distance, lateral_jerk, velocity), 96.0);
102+
}
103+
104+
TEST(path_shift_test, calc_shift_time_from_jerk)
105+
{
106+
constexpr double epsilon = 1e-6;
107+
108+
using autoware::motion_utils::calc_shift_time_from_jerk;
109+
110+
double lateral_distance = 0.0;
111+
double lateral_jerk = 0.0;
112+
double lateral_acceleration = 0.0;
113+
114+
// Condition: zero lateral jerk
115+
EXPECT_DOUBLE_EQ(
116+
calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 1.0e10);
117+
118+
// Condition: zero lateral acceleration
119+
lateral_jerk = -2.0;
120+
EXPECT_DOUBLE_EQ(
121+
calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 1.0e10);
122+
123+
// Condition: zero lateral distance
124+
lateral_acceleration = -4.0;
125+
EXPECT_DOUBLE_EQ(
126+
calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 0.0);
127+
128+
// Condition: random (TODO: use DOUBLE_EQ in future. currently not precise enough)
129+
lateral_distance = 80.0;
130+
EXPECT_NEAR(
131+
calc_shift_time_from_jerk(lateral_distance, lateral_jerk, lateral_acceleration), 11.16515139,
132+
epsilon);
133+
}
134+
135+
TEST(path_shift_test, calc_jerk_from_lat_lon_distance)
136+
{
137+
using autoware::motion_utils::calc_jerk_from_lat_lon_distance;
138+
139+
double lateral_distance = 0.0;
140+
double longitudinal_distance = 100.0;
141+
double velocity = 10.0;
142+
143+
// Condition: zero lateral distance
144+
EXPECT_DOUBLE_EQ(
145+
calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.0);
146+
147+
// Condition: zero velocity
148+
lateral_distance = 5.0;
149+
velocity = 0.0;
150+
EXPECT_DOUBLE_EQ(
151+
calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.0);
152+
153+
// Condition: zero longitudinal distance
154+
longitudinal_distance = 0.0;
155+
velocity = 10.0;
156+
EXPECT_DOUBLE_EQ(
157+
calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 1.6 * 1e14);
158+
159+
// Condition: random
160+
longitudinal_distance = 100.0;
161+
EXPECT_DOUBLE_EQ(
162+
calc_jerk_from_lat_lon_distance(lateral_distance, longitudinal_distance, velocity), 0.16);
163+
}

0 commit comments

Comments
 (0)