-
Notifications
You must be signed in to change notification settings - Fork 676
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat!: move diagnostics_module from localization_util to unverse_utils (
#9714) * feat!: move diagnostics_module from localization_util to unverse_utils Signed-off-by: kminoda <[email protected]> * remove diagnostics module from localization_util Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * minor fix in pose_initializer Signed-off-by: kminoda <[email protected]> * add test Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * remove unnecessary declaration Signed-off-by: kminoda <[email protected]> * module -> interface Signed-off-by: kminoda <[email protected]> * remove unnecessary equal expression Signed-off-by: kminoda <[email protected]> * revert the remove of template function Signed-off-by: kminoda <[email protected]> * style(pre-commit): autofix * use overload instead Signed-off-by: kminoda <[email protected]> * include what you use -- test_diagnostics_interface.cpp Signed-off-by: kminoda <[email protected]> --------- Signed-off-by: kminoda <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
- Loading branch information
1 parent
421ec7d
commit 16d5cb1
Showing
19 changed files
with
261 additions
and
87 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
179 changes: 179 additions & 0 deletions
179
common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,179 @@ | ||
| // Copyright 2024 Autoware Foundation | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #include "autoware/universe_utils/ros/diagnostics_interface.hpp" | ||
|
|
||
| #include <rclcpp/rclcpp.hpp> | ||
|
|
||
| #include <diagnostic_msgs/msg/diagnostic_array.hpp> | ||
| #include <diagnostic_msgs/msg/diagnostic_status.hpp> | ||
| #include <diagnostic_msgs/msg/key_value.hpp> | ||
|
|
||
| #include <gtest/gtest.h> | ||
|
|
||
| #include <memory> | ||
| #include <string> | ||
|
|
||
| using autoware::universe_utils::DiagnosticInterface; | ||
|
|
||
| class TestDiagnosticInterface : public ::testing::Test | ||
| { | ||
| protected: | ||
| void SetUp() override | ||
| { | ||
| // Create a test node | ||
| node_ = std::make_shared<rclcpp::Node>("test_diagnostics_interface"); | ||
| } | ||
|
|
||
| // Automatically destroyed at the end of each test | ||
| std::shared_ptr<rclcpp::Node> node_; | ||
| }; | ||
|
|
||
| /* | ||
| * Test clear(): | ||
| * Verify that calling clear() resets DiagnosticStatus values, level, and message. | ||
| */ | ||
| TEST_F(TestDiagnosticInterface, ClearTest) | ||
| { | ||
| DiagnosticInterface module(node_.get(), "test_diagnostic"); | ||
|
|
||
| // Add some key-value pairs and update level/message | ||
| module.add_key_value("param1", 42); | ||
| module.update_level_and_message( | ||
| diagnostic_msgs::msg::DiagnosticStatus::WARN, "Something is not OK"); | ||
|
|
||
| // Call clear() | ||
| module.clear(); | ||
|
|
||
| // After calling clear(), publish and check the contents of the message | ||
| diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr received_msg; | ||
| auto sub = node_->create_subscription<diagnostic_msgs::msg::DiagnosticArray>( | ||
| "/diagnostics", 10, | ||
| [&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) { received_msg = msg; }); | ||
|
|
||
| // Publish the message | ||
| module.publish(node_->now()); | ||
|
|
||
| // Spin to allow the subscriber to receive messages | ||
| rclcpp::spin_some(node_); | ||
|
|
||
| ASSERT_NE(nullptr, received_msg); | ||
| ASSERT_FALSE(received_msg->status.empty()); | ||
| const auto & status = received_msg->status.front(); | ||
|
|
||
| // After clear(), key-value pairs should be empty | ||
| EXPECT_TRUE(status.values.empty()); | ||
|
|
||
| // After clear(), level should be OK (=0) | ||
| EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::OK); | ||
|
|
||
| // After clear(), message is empty internally, | ||
| // but "OK" is set during publishing when level == OK | ||
| EXPECT_EQ(status.message, "OK"); | ||
| } | ||
|
|
||
| /* | ||
| * Test add_key_value(): | ||
| * Verify that adding the same key updates its value instead of adding a duplicate. | ||
| */ | ||
| TEST_F(TestDiagnosticInterface, AddKeyValueTest) | ||
| { | ||
| DiagnosticInterface module(node_.get(), "test_diagnostic"); | ||
|
|
||
| // Call the template version of add_key_value() with different types | ||
| module.add_key_value("string_key", std::string("initial_value")); | ||
| module.add_key_value("int_key", 123); | ||
| module.add_key_value("bool_key", true); | ||
|
|
||
| // Overwrite the value for "string_key" | ||
| module.add_key_value("string_key", std::string("updated_value")); | ||
|
|
||
| // Capture the published message to verify its contents | ||
| diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr received_msg; | ||
| auto sub = node_->create_subscription<diagnostic_msgs::msg::DiagnosticArray>( | ||
| "/diagnostics", 10, | ||
| [&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) { received_msg = msg; }); | ||
| module.publish(node_->now()); | ||
| rclcpp::spin_some(node_); | ||
|
|
||
| ASSERT_NE(nullptr, received_msg); | ||
| ASSERT_FALSE(received_msg->status.empty()); | ||
| const auto & status = received_msg->status.front(); | ||
|
|
||
| // Expect 3 key-value pairs | ||
| ASSERT_EQ(status.values.size(), 3U); | ||
|
|
||
| // Helper lambda to find a value by key | ||
| auto find_value = [&](const std::string & key) -> std::string { | ||
| for (const auto & kv : status.values) { | ||
| if (kv.key == key) { | ||
| return kv.value; | ||
| } | ||
| } | ||
| return ""; | ||
| }; | ||
|
|
||
| EXPECT_EQ(find_value("string_key"), "updated_value"); | ||
| EXPECT_EQ(find_value("int_key"), "123"); | ||
| EXPECT_EQ(find_value("bool_key"), "True"); | ||
|
|
||
| // Status level should still be OK | ||
| EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::OK); | ||
| // Message should be "OK" | ||
| EXPECT_EQ(status.message, "OK"); | ||
| } | ||
|
|
||
| /* | ||
| * Test update_level_and_message(): | ||
| * Verify that the level is updated to the highest severity and | ||
| * that messages are concatenated if level > OK. | ||
| */ | ||
| TEST_F(TestDiagnosticInterface, UpdateLevelAndMessageTest) | ||
| { | ||
| DiagnosticInterface module(node_.get(), "test_diagnostic"); | ||
|
|
||
| // Initial status is level=OK(0), message="" | ||
| module.update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::OK, "Initial OK"); | ||
| // Update to WARN (1) | ||
| module.update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::WARN, "Low battery"); | ||
| // Update to ERROR (2) | ||
| module.update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Sensor failure"); | ||
| // Another WARN (1) after ERROR should not downgrade the overall level | ||
| module.update_level_and_message( | ||
| diagnostic_msgs::msg::DiagnosticStatus::WARN, "Should not override error"); | ||
|
|
||
| diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr received_msg; | ||
| auto sub = node_->create_subscription<diagnostic_msgs::msg::DiagnosticArray>( | ||
| "/diagnostics", 10, | ||
| [&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) { received_msg = msg; }); | ||
|
|
||
| module.publish(node_->now()); | ||
| rclcpp::spin_some(node_); | ||
|
|
||
| ASSERT_NE(nullptr, received_msg); | ||
| ASSERT_FALSE(received_msg->status.empty()); | ||
| const auto & status = received_msg->status.front(); | ||
|
|
||
| // Final level should be ERROR (2) | ||
| EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); | ||
|
|
||
| // The message should contain all parts that were added when level > OK. | ||
| // Thus, we expect something like: | ||
| // "Low battery; Sensor failure; Should not override error" | ||
| const std::string & final_message = status.message; | ||
| EXPECT_FALSE(final_message.find("Initial OK") != std::string::npos); | ||
| EXPECT_TRUE(final_message.find("Low battery") != std::string::npos); | ||
| EXPECT_TRUE(final_message.find("Sensor failure") != std::string::npos); | ||
| EXPECT_TRUE(final_message.find("Should not override error") != std::string::npos); | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.