Skip to content

Commit e7a8268

Browse files
kminodakminoda
authored
test(map_based_prediction): add test for path_generator (#7056)
1 parent d6e8012 commit e7a8268

File tree

3 files changed

+260
-0
lines changed

3 files changed

+260
-0
lines changed

‎perception/map_based_prediction/CMakeLists.txt

+16
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,22 @@ rclcpp_components_register_node(map_based_prediction_node
2626
EXECUTABLE map_based_prediction
2727
)
2828

29+
## Tests
30+
if(BUILD_TESTING)
31+
find_package(ament_cmake_ros REQUIRED)
32+
list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify)
33+
34+
find_package(ament_lint_auto REQUIRED)
35+
ament_lint_auto_find_test_dependencies()
36+
37+
file(GLOB_RECURSE test_files test/**/*.cpp)
38+
ament_add_ros_isolated_gtest(test_map_based_prediction ${test_files})
39+
40+
target_link_libraries(test_map_based_prediction
41+
map_based_prediction_node
42+
)
43+
endif()
44+
2945
ament_auto_package(
3046
INSTALL_TO_SHARE
3147
config

‎perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp

+218
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,218 @@
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 "map_based_prediction/path_generator.hpp"
16+
17+
#include <glog/logging.h>
18+
#include <gtest/gtest.h>
19+
20+
using autoware_auto_perception_msgs::msg::ObjectClassification;
21+
using autoware_auto_perception_msgs::msg::PredictedObject;
22+
using autoware_auto_perception_msgs::msg::PredictedObjectKinematics;
23+
using autoware_auto_perception_msgs::msg::PredictedObjects;
24+
using autoware_auto_perception_msgs::msg::PredictedPath;
25+
using autoware_auto_perception_msgs::msg::TrackedObject;
26+
using autoware_auto_perception_msgs::msg::TrackedObjectKinematics;
27+
using autoware_auto_perception_msgs::msg::TrackedObjects;
28+
29+
TrackedObject generate_static_object(const int label)
30+
{
31+
ObjectClassification classification;
32+
classification.probability = 1.0;
33+
classification.label = label;
34+
35+
TrackedObjectKinematics kinematics;
36+
kinematics.pose_with_covariance = geometry_msgs::msg::PoseWithCovariance(); // At origin
37+
kinematics.twist_with_covariance = geometry_msgs::msg::TwistWithCovariance(); // Not moving
38+
kinematics.acceleration_with_covariance =
39+
geometry_msgs::msg::AccelWithCovariance(); // Not accelerating
40+
kinematics.orientation_availability = TrackedObjectKinematics::UNAVAILABLE;
41+
42+
TrackedObject tracked_object;
43+
tracked_object.object_id = unique_identifier_msgs::msg::UUID();
44+
tracked_object.existence_probability = 1.0;
45+
tracked_object.classification.push_back(classification);
46+
tracked_object.kinematics = kinematics;
47+
48+
return tracked_object;
49+
}
50+
51+
TEST(PathGenerator, test_generatePathForNonVehicleObject)
52+
{
53+
// Generate Path generator
54+
const double prediction_time_horizon = 10.0;
55+
const double lateral_control_time_horizon = 5.0;
56+
const double prediction_sampling_time_interval = 0.5;
57+
const double min_crosswalk_user_velocity = 0.1;
58+
const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator(
59+
prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval,
60+
min_crosswalk_user_velocity);
61+
62+
// Generate pedestrian object
63+
TrackedObject tracked_object = generate_static_object(ObjectClassification::PEDESTRIAN);
64+
65+
// Generate predicted path
66+
const PredictedPath predicted_path =
67+
path_generator.generatePathForNonVehicleObject(tracked_object);
68+
69+
// Check
70+
EXPECT_FALSE(predicted_path.path.empty());
71+
EXPECT_EQ(predicted_path.path[0].position.x, 0.0);
72+
EXPECT_EQ(predicted_path.path[0].position.y, 0.0);
73+
EXPECT_EQ(predicted_path.path[0].position.z, 0.0);
74+
}
75+
76+
TEST(PathGenerator, test_generatePathForLowSpeedVehicle)
77+
{
78+
// Generate Path generator
79+
const double prediction_time_horizon = 10.0;
80+
const double lateral_control_time_horizon = 5.0;
81+
const double prediction_sampling_time_interval = 0.5;
82+
const double min_crosswalk_user_velocity = 0.1;
83+
const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator(
84+
prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval,
85+
min_crosswalk_user_velocity);
86+
87+
// Generate dummy object
88+
TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR);
89+
90+
// Generate predicted path
91+
const PredictedPath predicted_path =
92+
path_generator.generatePathForLowSpeedVehicle(tracked_object);
93+
94+
// Check
95+
EXPECT_FALSE(predicted_path.path.empty());
96+
EXPECT_EQ(predicted_path.path[0].position.x, 0.0);
97+
EXPECT_EQ(predicted_path.path[0].position.y, 0.0);
98+
EXPECT_EQ(predicted_path.path[0].position.z, 0.0);
99+
}
100+
101+
TEST(PathGenerator, test_generatePathForOffLaneVehicle)
102+
{
103+
// Generate Path generator
104+
const double prediction_time_horizon = 10.0;
105+
const double lateral_control_time_horizon = 5.0;
106+
const double prediction_sampling_time_interval = 0.5;
107+
const double min_crosswalk_user_velocity = 0.1;
108+
const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator(
109+
prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval,
110+
min_crosswalk_user_velocity);
111+
112+
// Generate dummy object
113+
TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR);
114+
115+
const PredictedPath predicted_path = path_generator.generatePathForOffLaneVehicle(tracked_object);
116+
117+
// Check
118+
EXPECT_FALSE(predicted_path.path.empty());
119+
EXPECT_EQ(predicted_path.path[0].position.x, 0.0);
120+
EXPECT_EQ(predicted_path.path[0].position.y, 0.0);
121+
EXPECT_EQ(predicted_path.path[0].position.z, 0.0);
122+
}
123+
124+
TEST(PathGenerator, test_generatePathForOnLaneVehicle)
125+
{
126+
// Generate Path generator
127+
const double prediction_time_horizon = 10.0;
128+
const double lateral_control_time_horizon = 5.0;
129+
const double prediction_sampling_time_interval = 0.5;
130+
const double min_crosswalk_user_velocity = 0.1;
131+
const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator(
132+
prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval,
133+
min_crosswalk_user_velocity);
134+
135+
// Generate dummy object
136+
TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR);
137+
138+
// Generate reference path
139+
map_based_prediction::PosePath ref_paths;
140+
geometry_msgs::msg::Pose pose;
141+
pose.position.x = 0.0;
142+
pose.position.y = 0.0;
143+
pose.position.z = 0.0;
144+
ref_paths.push_back(pose);
145+
146+
// Generate predicted path
147+
const PredictedPath predicted_path =
148+
path_generator.generatePathForOnLaneVehicle(tracked_object, ref_paths);
149+
150+
// Check
151+
EXPECT_FALSE(predicted_path.path.empty());
152+
EXPECT_EQ(predicted_path.path[0].position.x, 0.0);
153+
EXPECT_EQ(predicted_path.path[0].position.y, 0.0);
154+
EXPECT_EQ(predicted_path.path[0].position.z, 0.0);
155+
}
156+
157+
TEST(PathGenerator, test_generatePathForCrosswalkUser)
158+
{
159+
// Generate Path generator
160+
const double prediction_time_horizon = 10.0;
161+
const double lateral_control_time_horizon = 5.0;
162+
const double prediction_sampling_time_interval = 0.5;
163+
const double min_crosswalk_user_velocity = 0.1;
164+
const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator(
165+
prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval,
166+
min_crosswalk_user_velocity);
167+
168+
// Generate dummy object
169+
TrackedObject tracked_object = generate_static_object(ObjectClassification::PEDESTRIAN);
170+
171+
// Generage dummy crosswalk
172+
map_based_prediction::CrosswalkEdgePoints reachable_crosswalk;
173+
reachable_crosswalk.front_center_point << 0.0, 0.0;
174+
reachable_crosswalk.front_right_point << 1.0, 0.0;
175+
reachable_crosswalk.front_left_point << -1.0, 0.0;
176+
reachable_crosswalk.back_center_point << 0.0, 1.0;
177+
reachable_crosswalk.back_right_point << 1.0, 1.0;
178+
reachable_crosswalk.back_left_point << -1.0, 1.0;
179+
180+
// Generate predicted path
181+
const PredictedPath predicted_path =
182+
path_generator.generatePathForCrosswalkUser(tracked_object, reachable_crosswalk);
183+
184+
// Check
185+
EXPECT_FALSE(predicted_path.path.empty());
186+
EXPECT_EQ(predicted_path.path[0].position.x, 0.0);
187+
EXPECT_EQ(predicted_path.path[0].position.y, 0.0);
188+
EXPECT_EQ(predicted_path.path[0].position.z, 0.0);
189+
}
190+
191+
TEST(PathGenerator, test_generatePathToTargetPoint)
192+
{
193+
// Generate Path generator
194+
const double prediction_time_horizon = 10.0;
195+
const double lateral_control_time_horizon = 5.0;
196+
const double prediction_sampling_time_interval = 0.5;
197+
const double min_crosswalk_user_velocity = 0.1;
198+
const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator(
199+
prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval,
200+
min_crosswalk_user_velocity);
201+
202+
// Generate dummy object
203+
TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR);
204+
205+
// Generate target point
206+
Eigen::Vector2d target_point;
207+
target_point << 0.0, 0.0;
208+
209+
// Generate predicted path
210+
const PredictedPath predicted_path =
211+
path_generator.generatePathToTargetPoint(tracked_object, target_point);
212+
213+
// Check
214+
EXPECT_FALSE(predicted_path.path.empty());
215+
EXPECT_EQ(predicted_path.path[0].position.x, 0.0);
216+
EXPECT_EQ(predicted_path.path[0].position.y, 0.0);
217+
EXPECT_EQ(predicted_path.path[0].position.z, 0.0);
218+
}

‎perception/map_based_prediction/test/test_map_based_prediction.cpp

+26
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
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 <rclcpp/rclcpp.hpp>
16+
17+
#include <gtest/gtest.h>
18+
19+
int main(int argc, char * argv[])
20+
{
21+
testing::InitGoogleTest(&argc, argv);
22+
rclcpp::init(argc, argv);
23+
bool result = RUN_ALL_TESTS();
24+
rclcpp::shutdown();
25+
return result;
26+
}

0 commit comments

Comments
 (0)