12
12
// See the License for the specific language governing permissions and
13
13
// limitations under the License.
14
14
15
- #include < gtest/gtest.h>
15
+ #include " autoware/universe_utils/ros/diagnostics_module.hpp"
16
+
16
17
#include < rclcpp/rclcpp.hpp>
17
18
18
19
#include < diagnostic_msgs/msg/diagnostic_array.hpp>
19
20
#include < diagnostic_msgs/msg/diagnostic_status.hpp>
20
21
#include < diagnostic_msgs/msg/key_value.hpp>
21
22
22
- #include " autoware/universe_utils/ros/diagnostics_module.hpp "
23
+ #include < gtest/gtest.h >
23
24
24
25
using autoware::universe_utils::DiagnosticsModule;
25
26
@@ -47,21 +48,16 @@ TEST_F(TestDiagnosticsModule, ClearTest)
47
48
// Add some key-value pairs and update level/message
48
49
module.add_key_value (" param1" , 42 );
49
50
module.update_level_and_message (
50
- diagnostic_msgs::msg::DiagnosticStatus::WARN,
51
- " Something is not OK"
52
- );
53
-
51
+ diagnostic_msgs::msg::DiagnosticStatus::WARN, " Something is not OK" );
52
+
54
53
// Call clear()
55
54
module.clear ();
56
55
57
56
// After calling clear(), publish and check the contents of the message
58
57
diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr received_msg;
59
58
auto sub = node_->create_subscription <diagnostic_msgs::msg::DiagnosticArray>(
60
59
" /diagnostics" , 10 ,
61
- [&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) {
62
- received_msg = msg;
63
- }
64
- );
60
+ [&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) { received_msg = msg; });
65
61
66
62
// Publish the message
67
63
module.publish (node_->now ());
@@ -104,10 +100,7 @@ TEST_F(TestDiagnosticsModule, AddKeyValueTest)
104
100
diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr received_msg;
105
101
auto sub = node_->create_subscription <diagnostic_msgs::msg::DiagnosticArray>(
106
102
" /diagnostics" , 10 ,
107
- [&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) {
108
- received_msg = msg;
109
- }
110
- );
103
+ [&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) { received_msg = msg; });
111
104
module.publish (node_->now ());
112
105
rclcpp::spin_some (node_);
113
106
@@ -138,7 +131,6 @@ TEST_F(TestDiagnosticsModule, AddKeyValueTest)
138
131
EXPECT_EQ (status.message , " OK" );
139
132
}
140
133
141
-
142
134
/*
143
135
* Test update_level_and_message():
144
136
* Verify that the level is updated to the highest severity and
@@ -149,33 +141,19 @@ TEST_F(TestDiagnosticsModule, UpdateLevelAndMessageTest)
149
141
DiagnosticsModule module (node_.get (), " test_diagnostic" );
150
142
151
143
// Initial status is level=OK(0), message=""
152
- module.update_level_and_message (
153
- diagnostic_msgs::msg::DiagnosticStatus::OK,
154
- " Initial OK"
155
- );
144
+ module.update_level_and_message (diagnostic_msgs::msg::DiagnosticStatus::OK, " Initial OK" );
156
145
// Update to WARN (1)
157
- module.update_level_and_message (
158
- diagnostic_msgs::msg::DiagnosticStatus::WARN,
159
- " Low battery"
160
- );
146
+ module.update_level_and_message (diagnostic_msgs::msg::DiagnosticStatus::WARN, " Low battery" );
161
147
// Update to ERROR (2)
162
- module.update_level_and_message (
163
- diagnostic_msgs::msg::DiagnosticStatus::ERROR,
164
- " Sensor failure"
165
- );
148
+ module.update_level_and_message (diagnostic_msgs::msg::DiagnosticStatus::ERROR, " Sensor failure" );
166
149
// Another WARN (1) after ERROR should not downgrade the overall level
167
150
module.update_level_and_message (
168
- diagnostic_msgs::msg::DiagnosticStatus::WARN,
169
- " Should not override error"
170
- );
151
+ diagnostic_msgs::msg::DiagnosticStatus::WARN, " Should not override error" );
171
152
172
153
diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr received_msg;
173
154
auto sub = node_->create_subscription <diagnostic_msgs::msg::DiagnosticArray>(
174
155
" /diagnostics" , 10 ,
175
- [&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) {
176
- received_msg = msg;
177
- }
178
- );
156
+ [&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) { received_msg = msg; });
179
157
180
158
module.publish (node_->now ());
181
159
rclcpp::spin_some (node_);
@@ -195,4 +173,4 @@ TEST_F(TestDiagnosticsModule, UpdateLevelAndMessageTest)
195
173
EXPECT_TRUE (final_message.find (" Low battery" ) != std::string::npos);
196
174
EXPECT_TRUE (final_message.find (" Sensor failure" ) != std::string::npos);
197
175
EXPECT_TRUE (final_message.find (" Should not override error" ) != std::string::npos);
198
- }
176
+ }
0 commit comments