1
- // Copyright 2024 Tier IV, Inc.
1
+ // Copyright 2025 Tier IV, Inc.
2
2
//
3
3
// Licensed under the Apache License, Version 2.0 (the "License");
4
4
// you may not use this file except in compliance with the License.
30
30
#include < iostream>
31
31
#include < memory>
32
32
#include < string>
33
+ #include < tuple>
33
34
#include < utility>
34
35
#include < vector>
35
36
@@ -53,9 +54,12 @@ class EvalTest : public ::testing::Test
53
54
rclcpp::NodeOptions options;
54
55
const auto share_dir =
55
56
ament_index_cpp::get_package_share_directory (" autoware_planning_evaluator" );
57
+ const auto autoware_test_utils_dir =
58
+ ament_index_cpp::get_package_share_directory (" autoware_test_utils" );
56
59
options.arguments (
57
- {" --ros-args" , " --params-file" , share_dir + " /config/planning_evaluator.param.yaml" , " -p" ,
58
- " output_metrics:=false" });
60
+ {" --ros-args" , " -p" , " output_metrics:=false" , " --params-file" ,
61
+ share_dir + " /config/planning_evaluator.param.yaml" , " --params-file" ,
62
+ autoware_test_utils_dir + " /config/test_vehicle_info.param.yaml" });
59
63
60
64
dummy_node = std::make_shared<rclcpp::Node>(" planning_evaluator_test_node" );
61
65
eval_node = std::make_shared<EvalNode>(options);
@@ -83,6 +87,7 @@ class EvalTest : public ::testing::Test
83
87
dummy_node, " /planning_evaluator/input/modified_goal" , 1 );
84
88
85
89
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(dummy_node);
90
+ vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils (*eval_node).getVehicleInfo ();
86
91
publishEgoPose (0.0 , 0.0 , 0.0 );
87
92
}
88
93
@@ -118,6 +123,25 @@ class EvalTest : public ::testing::Test
118
123
return t;
119
124
}
120
125
126
+ Trajectory makeTrajectory (const std::vector<std::tuple<double , double , double >> & traj)
127
+ {
128
+ Trajectory t;
129
+ t.header .frame_id = " map" ;
130
+ TrajectoryPoint p;
131
+ for (const std::tuple<double , double , double > & point : traj) {
132
+ p.pose .position .x = std::get<0 >(point);
133
+ p.pose .position .y = std::get<1 >(point);
134
+ tf2::Quaternion q;
135
+ q.setRPY (0.0 , 0.0 , std::get<2 >(point));
136
+ p.pose .orientation .x = q.x ();
137
+ p.pose .orientation .y = q.y ();
138
+ p.pose .orientation .z = q.z ();
139
+ p.pose .orientation .w = q.w ();
140
+ t.points .push_back (p);
141
+ }
142
+ return t;
143
+ }
144
+
121
145
void publishTrajectory (const Trajectory & traj)
122
146
{
123
147
traj_pub_->publish (traj);
@@ -213,6 +237,9 @@ class EvalTest : public ::testing::Test
213
237
rclcpp::Subscription<MetricArrayMsg>::SharedPtr metric_sub_;
214
238
// TF broadcaster
215
239
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
240
+
241
+ public:
242
+ autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
216
243
};
217
244
218
245
TEST_F (EvalTest, TestCurvature)
@@ -250,6 +277,16 @@ TEST_F(EvalTest, TestRelativeAngle)
250
277
EXPECT_DOUBLE_EQ (publishTrajectoryAndGetMetric (t), 0.0 );
251
278
}
252
279
280
+ TEST_F (EvalTest, TestResampledRelativeAngle)
281
+ {
282
+ setTargetMetric (planning_diagnostics::Metric::resampled_relative_angle);
283
+ Trajectory t = makeTrajectory ({{0.0 , 0.0 , 0.0 }, {vehicle_info_.vehicle_length_m , 0.0 , 0.0 }});
284
+ EXPECT_DOUBLE_EQ (publishTrajectoryAndGetMetric (t), 0.0 );
285
+ t = makeTrajectory (
286
+ {{0.0 , 0.0 , 0.0 }, {vehicle_info_.vehicle_length_m , vehicle_info_.vehicle_length_m , M_PI_4}});
287
+ EXPECT_DOUBLE_EQ (publishTrajectoryAndGetMetric (t), M_PI_4);
288
+ }
289
+
253
290
TEST_F (EvalTest, TestLength)
254
291
{
255
292
setTargetMetric (planning_diagnostics::Metric::length);
0 commit comments