Skip to content

Commit 582d9f1

Browse files
xtk8532704xtk8532704
authored
test(autoware_control_evaluator): add test for autoware_control_evaluator. (#9114)
* init Signed-off-by: xtk8532704 <1041084556@qq.com> * tmp save. Signed-off-by: xtk8532704 <1041084556@qq.com> * save, there is a bug Signed-off-by: xtk8532704 <1041084556@qq.com> * update package.xml Signed-off-by: xtk8532704 <1041084556@qq.com> * coverage rate 64.5 Signed-off-by: xtk8532704 <1041084556@qq.com> * remove comments. Signed-off-by: xtk8532704 <1041084556@qq.com> --------- Signed-off-by: xtk8532704 <1041084556@qq.com>
1 parent 92868c0 commit 582d9f1

File tree

4 files changed

+210
-3
lines changed

4 files changed

+210
-3
lines changed

‎evaluator/autoware_control_evaluator/CMakeLists.txt

+9
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,15 @@ rclcpp_components_register_node(control_evaluator_node
1616
EXECUTABLE control_evaluator
1717
)
1818

19+
if(BUILD_TESTING)
20+
ament_add_ros_isolated_gtest(test_control_evaluator
21+
test/test_control_evaluator_node.cpp
22+
)
23+
target_link_libraries(test_control_evaluator
24+
control_evaluator_node
25+
)
26+
endif()
27+
1928

2029
ament_auto_package(
2130
INSTALL_TO_SHARE

‎evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp

-2
Original file line numberDiff line numberDiff line change
@@ -56,8 +56,6 @@ class ControlEvaluatorNode : public rclcpp::Node
5656
const Trajectory & traj, const Point & ego_point);
5757
DiagnosticStatus generateYawDeviationDiagnosticStatus(
5858
const Trajectory & traj, const Pose & ego_pose);
59-
std::optional<DiagnosticStatus> generateStopDiagnosticStatus(
60-
const DiagnosticArray & diag, const std::string & function_name);
6159

6260
DiagnosticStatus generateAEBDiagnosticStatus(const DiagnosticStatus & diag);
6361
DiagnosticStatus generateLaneletDiagnosticStatus(const Pose & ego_pose) const;

‎evaluator/autoware_control_evaluator/package.xml

+4-1
Original file line numberDiff line numberDiff line change
@@ -20,12 +20,15 @@
2020
<depend>autoware_motion_utils</depend>
2121
<depend>autoware_planning_msgs</depend>
2222
<depend>autoware_route_handler</depend>
23+
<depend>autoware_test_utils</depend>
2324
<depend>autoware_universe_utils</depend>
2425
<depend>diagnostic_msgs</depend>
26+
<depend>nav_msgs</depend>
2527
<depend>pluginlib</depend>
2628
<depend>rclcpp</depend>
2729
<depend>rclcpp_components</depend>
28-
<!-- <depend>nav_msgs</depend> -->
30+
<depend>tf2</depend>
31+
<depend>tf2_ros</depend>
2932

3033
<test_depend>ament_cmake_ros</test_depend>
3134
<test_depend>ament_lint_auto</test_depend>

‎evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp

+197
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,197 @@
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 "ament_index_cpp/get_package_share_directory.hpp"
16+
#include "gtest/gtest.h"
17+
#include "rclcpp/rclcpp.hpp"
18+
#include "rclcpp/time.hpp"
19+
#include "tf2_ros/transform_broadcaster.h"
20+
21+
#include <autoware/control_evaluator/control_evaluator_node.hpp>
22+
23+
#include "autoware_planning_msgs/msg/trajectory.hpp"
24+
#include "diagnostic_msgs/msg/diagnostic_array.hpp"
25+
#include "geometry_msgs/msg/transform_stamped.hpp"
26+
27+
#include "boost/lexical_cast.hpp"
28+
29+
#include <tf2/LinearMath/Quaternion.h>
30+
31+
#include <memory>
32+
#include <string>
33+
#include <utility>
34+
#include <vector>
35+
36+
using EvalNode = control_diagnostics::ControlEvaluatorNode;
37+
using Trajectory = autoware_planning_msgs::msg::Trajectory;
38+
using TrajectoryPoint = autoware_planning_msgs::msg::TrajectoryPoint;
39+
using diagnostic_msgs::msg::DiagnosticArray;
40+
using nav_msgs::msg::Odometry;
41+
42+
constexpr double epsilon = 1e-6;
43+
44+
class EvalTest : public ::testing::Test
45+
{
46+
protected:
47+
void SetUp() override
48+
{
49+
rclcpp::init(0, nullptr);
50+
51+
rclcpp::NodeOptions options;
52+
const auto share_dir =
53+
ament_index_cpp::get_package_share_directory("autoware_control_evaluator");
54+
55+
dummy_node = std::make_shared<rclcpp::Node>("control_evaluator_test_node");
56+
eval_node = std::make_shared<EvalNode>(options);
57+
// Enable all logging in the node
58+
auto ret = rcutils_logging_set_logger_level(
59+
dummy_node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
60+
if (ret != RCUTILS_RET_OK) {
61+
std::cerr << "Failed to set logging severity to DEBUG\n";
62+
}
63+
ret = rcutils_logging_set_logger_level(
64+
eval_node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
65+
if (ret != RCUTILS_RET_OK) {
66+
std::cerr << "Failed to set logging severity to DEBUG\n";
67+
}
68+
traj_pub_ =
69+
rclcpp::create_publisher<Trajectory>(dummy_node, "/control_evaluator/input/trajectory", 1);
70+
odom_pub_ =
71+
rclcpp::create_publisher<Odometry>(dummy_node, "/control_evaluator/input/odometry", 1);
72+
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(dummy_node);
73+
publishEgoPose(0.0, 0.0, 0.0);
74+
}
75+
76+
~EvalTest() override { rclcpp::shutdown(); }
77+
78+
void setTargetMetric(const std::string & metric_str)
79+
{
80+
const auto is_target_metric = [metric_str](const auto & status) {
81+
return status.name == metric_str;
82+
};
83+
metric_sub_ = rclcpp::create_subscription<DiagnosticArray>(
84+
dummy_node, "/control_evaluator/metrics", 1, [=](const DiagnosticArray::ConstSharedPtr msg) {
85+
const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric);
86+
if (it != msg->status.end()) {
87+
metric_value_ = boost::lexical_cast<double>(it->values[0].value);
88+
metric_updated_ = true;
89+
}
90+
});
91+
}
92+
93+
Trajectory makeTrajectory(const std::vector<std::pair<double, double>> & traj)
94+
{
95+
Trajectory t;
96+
t.header.frame_id = "map";
97+
TrajectoryPoint p;
98+
for (const std::pair<double, double> & point : traj) {
99+
p.pose.position.x = point.first;
100+
p.pose.position.y = point.second;
101+
t.points.push_back(p);
102+
}
103+
return t;
104+
}
105+
106+
void publishTrajectory(const Trajectory & traj)
107+
{
108+
traj_pub_->publish(traj);
109+
rclcpp::spin_some(eval_node);
110+
rclcpp::spin_some(dummy_node);
111+
rclcpp::sleep_for(std::chrono::milliseconds(100));
112+
}
113+
114+
double publishTrajectoryAndGetMetric(const Trajectory & traj)
115+
{
116+
metric_updated_ = false;
117+
traj_pub_->publish(traj);
118+
while (!metric_updated_) {
119+
rclcpp::spin_some(eval_node);
120+
rclcpp::spin_some(dummy_node);
121+
rclcpp::sleep_for(std::chrono::milliseconds(100));
122+
}
123+
return metric_value_;
124+
}
125+
126+
void publishEgoPose(const double x, const double y, const double yaw)
127+
{
128+
Odometry odom;
129+
odom.header.frame_id = "map";
130+
odom.header.stamp = dummy_node->now();
131+
odom.pose.pose.position.x = x;
132+
odom.pose.pose.position.y = y;
133+
odom.pose.pose.position.z = 0.0;
134+
tf2::Quaternion q;
135+
q.setRPY(0.0, 0.0, yaw);
136+
odom.pose.pose.orientation.x = q.x();
137+
odom.pose.pose.orientation.y = q.y();
138+
odom.pose.pose.orientation.z = q.z();
139+
odom.pose.pose.orientation.w = q.w();
140+
141+
odom_pub_->publish(odom);
142+
rclcpp::spin_some(eval_node);
143+
rclcpp::spin_some(dummy_node);
144+
rclcpp::sleep_for(std::chrono::milliseconds(100));
145+
}
146+
147+
// Latest metric value
148+
bool metric_updated_ = false;
149+
double metric_value_;
150+
// Node
151+
rclcpp::Node::SharedPtr dummy_node;
152+
EvalNode::SharedPtr eval_node;
153+
// Trajectory publishers
154+
rclcpp::Publisher<Trajectory>::SharedPtr traj_pub_;
155+
rclcpp::Publisher<Odometry>::SharedPtr odom_pub_;
156+
rclcpp::Subscription<DiagnosticArray>::SharedPtr metric_sub_;
157+
// TF broadcaster
158+
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
159+
};
160+
161+
TEST_F(EvalTest, TestYawDeviation)
162+
{
163+
auto setYaw = [](geometry_msgs::msg::Quaternion & msg, const double yaw_rad) {
164+
tf2::Quaternion q;
165+
q.setRPY(0.0, 0.0, yaw_rad);
166+
msg.x = q.x();
167+
msg.y = q.y();
168+
msg.z = q.z();
169+
msg.w = q.w();
170+
};
171+
setTargetMetric("yaw_deviation");
172+
Trajectory t = makeTrajectory({{0.0, 0.0}, {1.0, 0.0}});
173+
for (auto & p : t.points) {
174+
setYaw(p.pose.orientation, M_PI);
175+
}
176+
177+
publishEgoPose(0.0, 0.0, M_PI);
178+
EXPECT_NEAR(publishTrajectoryAndGetMetric(t), 0.0, epsilon);
179+
180+
publishEgoPose(0.0, 0.0, 0.0);
181+
EXPECT_NEAR(publishTrajectoryAndGetMetric(t), M_PI, epsilon);
182+
183+
publishEgoPose(0.0, 0.0, -M_PI);
184+
EXPECT_NEAR(publishTrajectoryAndGetMetric(t), 0.0, epsilon);
185+
}
186+
187+
TEST_F(EvalTest, TestLateralDeviation)
188+
{
189+
setTargetMetric("lateral_deviation");
190+
Trajectory t = makeTrajectory({{0.0, 0.0}, {1.0, 0.0}});
191+
192+
publishEgoPose(0.0, 0.0, 0.0);
193+
EXPECT_NEAR(publishTrajectoryAndGetMetric(t), 0.0, epsilon);
194+
195+
publishEgoPose(1.0, 1.0, 0.0);
196+
EXPECT_NEAR(publishTrajectoryAndGetMetric(t), 1.0, epsilon);
197+
}

0 commit comments

Comments
 (0)