diff --git a/diagnostic_updater/CMakeLists.txt b/diagnostic_updater/CMakeLists.txt index 358c6e5e5..cd723d2e6 100644 --- a/diagnostic_updater/CMakeLists.txt +++ b/diagnostic_updater/CMakeLists.txt @@ -84,10 +84,22 @@ if(BUILD_TESTING) "rclcpp_lifecycle" "std_msgs" ) + ament_add_gtest(status_msg_test test/status_msg_test.cpp) + target_include_directories(status_msg_test + PUBLIC + $ + $ + ) + ament_target_dependencies( + status_msg_test + "diagnostic_msgs" + "rclcpp" + ) find_package(ament_cmake_pytest REQUIRED) ament_add_pytest_test(diagnostic_updater_test.py "test/diagnostic_updater_test.py") ament_add_pytest_test(test_DiagnosticStatusWrapper.py "test/test_diagnostic_status_wrapper.py") + ament_add_pytest_test(status_msg_test.py "test/status_msg_test.py") endif() ament_python_install_package(${PROJECT_NAME}) diff --git a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py index ef4ac5805..dc776a701 100644 --- a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py +++ b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py @@ -247,6 +247,19 @@ def __init__(self, node, period=1.0): self.hwid = '' self.warn_nohwid_done = False + self.use_fqn_parameter = 'diagnostic_updater.use_fqn' + if self.node.has_parameter(self.use_fqn_parameter): + self.__use_fqn = self.node.get_parameter( + self.use_fqn_parameter).value + else: + self.__use_fqn = self.node.declare_parameter( + self.use_fqn_parameter, False).value + + if self.__use_fqn: + self.node_name = '/'.join([self.node.get_namespace(), self.node.get_name()]) + else: + self.node_name = self.node.get_name() + def update(self): """ Update the diagnostics. @@ -352,7 +365,7 @@ def publish(self, msg): da = DiagnosticArray() da.header.stamp = now.to_msg() # Add timestamp for ROS 0.10 for stat in msg: - stat.name = self.node.get_name() + ': ' + stat.name + stat.name = self.node_name + ': ' + stat.name db = DiagnosticStatus() db.name = stat.name db.message = stat.message diff --git a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp index 0c9a19ce8..a301ada59 100644 --- a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp +++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp @@ -406,6 +406,17 @@ class Updater : public DiagnosticTaskVector period_ = rclcpp::Duration::from_seconds(period); reset_timer(); + + constexpr const char * use_fqn_param_name = "diagnostic_updater.use_fqn"; + rclcpp::ParameterValue use_fqn_param; + if (parameters_interface->has_parameter(use_fqn_param_name)) { + use_fqn_param = parameters_interface->get_parameter(use_fqn_param_name).get_parameter_value(); + } else { + use_fqn_param = parameters_interface->declare_parameter( + use_fqn_param_name, rclcpp::ParameterValue(false)); + } + node_name_ = use_fqn_param.get() ? + base_interface->get_fully_qualified_name() : base_interface->get_name(); } /** diff --git a/diagnostic_updater/package.xml b/diagnostic_updater/package.xml index 7cf5923a4..b8f0db71c 100644 --- a/diagnostic_updater/package.xml +++ b/diagnostic_updater/package.xml @@ -32,6 +32,7 @@ ament_lint_common launch launch_testing + python3-pytest rclcpp_lifecycle diff --git a/diagnostic_updater/test/status_msg_test.cpp b/diagnostic_updater/test/status_msg_test.cpp new file mode 100644 index 000000000..678ff0965 --- /dev/null +++ b/diagnostic_updater/test/status_msg_test.cpp @@ -0,0 +1,141 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2023, Outrider Technologies, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +#include +#include + +#include + +#include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "diagnostic_updater/diagnostic_updater.hpp" + + +class MockUpdaterNode : public rclcpp::Node +{ +public: + explicit MockUpdaterNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("mock_updater_node", "test_namespace", options), + updater_(this) + { + updater_.setHardwareID("test_hardware_id"); + updater_.add("test_check", this, &MockUpdaterNode::update_diagnostics); + updater_.force_update(); + } + + void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) + { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "test message"); + } + + rclcpp::Publisher::SharedPtr diagnostic_array_publisher_; + diagnostic_updater::Updater updater_; +}; + +class ListenerNode : public rclcpp::Node +{ +public: + ListenerNode() + : Node("listener_node") + { + subscriber_ = + this->create_subscription( + "diagnostics", 10, + std::bind(&ListenerNode::diagnostics_callback, this, std::placeholders::_1)); + } + + diagnostic_msgs::msg::DiagnosticArray::SharedPtr get_diagnostic_array() const + { + return diagnostic_array_; + } + +private: + void diagnostics_callback(const diagnostic_msgs::msg::DiagnosticArray::SharedPtr msg) + { + diagnostic_array_ = msg; + } + + diagnostic_msgs::msg::DiagnosticArray::SharedPtr diagnostic_array_; + rclcpp::Subscription::SharedPtr + subscriber_; +}; + +TEST(TestStatusMsg, test_name) { + auto mock_node = std::make_shared(); + auto listener_node = std::make_shared(); + + while (rclcpp::ok() && !listener_node->get_diagnostic_array()) { + rclcpp::spin_some(mock_node); + rclcpp::spin_some(listener_node); + } + + const auto diagnostic_array = listener_node->get_diagnostic_array(); + ASSERT_NE(nullptr, diagnostic_array); + ASSERT_EQ(1u, diagnostic_array->status.size()); + EXPECT_EQ("mock_updater_node: test_check", diagnostic_array->status[0].name); + EXPECT_EQ("test_hardware_id", diagnostic_array->status[0].hardware_id); + EXPECT_EQ("test message", diagnostic_array->status[0].message); + EXPECT_EQ(diagnostic_msgs::msg::DiagnosticStatus::OK, diagnostic_array->status[0].level); +} + +TEST(TestStatusMsg, test_fully_qualified_name) { + rclcpp::NodeOptions options; + options.append_parameter_override("diagnostic_updater.use_fqn", true); + + auto mock_node = std::make_shared(options); + auto listener_node = std::make_shared(); + + while (rclcpp::ok() && !listener_node->get_diagnostic_array()) { + rclcpp::spin_some(mock_node); + rclcpp::spin_some(listener_node); + } + + const auto diagnostic_array = listener_node->get_diagnostic_array(); + ASSERT_NE(nullptr, diagnostic_array); + ASSERT_EQ(1u, diagnostic_array->status.size()); + EXPECT_EQ("/test_namespace/mock_updater_node: test_check", diagnostic_array->status[0].name); + EXPECT_EQ("test_hardware_id", diagnostic_array->status[0].hardware_id); + EXPECT_EQ("test message", diagnostic_array->status[0].message); + EXPECT_EQ(diagnostic_msgs::msg::DiagnosticStatus::OK, diagnostic_array->status[0].level); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + const int ret = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + return ret; +} diff --git a/diagnostic_updater/test/status_msg_test.py b/diagnostic_updater/test/status_msg_test.py new file mode 100644 index 000000000..981c0663e --- /dev/null +++ b/diagnostic_updater/test/status_msg_test.py @@ -0,0 +1,77 @@ +import pytest +import rclpy +import unittest + +from diagnostic_updater import Updater +from launch import LaunchDescription +from launch_ros.actions import Node +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus +from launch_testing.actions import ReadyToTest + + +class TestProcessOutput(unittest.TestCase): + + @ classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @ classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.node = rclpy.create_node('listener_node') + self.subscriber = self.node.create_subscription( + DiagnosticArray, + '/diagnostics', + lambda msg: self.received_status.append(msg.status[0]), + 1) + self.received_status = [] + self.updater_node_name = 'updater_node' + + def tearDown(self): + self.node.destroy_node() + + def update_diagnostics(self, stat): + stat.summary(DiagnosticStatus.OK, 'test') + return stat + + def test_name(self): + updater_node = rclpy.create_node(self.updater_node_name) + updater = Updater(updater_node) + updater.setHardwareID('hardware_test') + updater.add('test check', self.update_diagnostics) + updater_node.create_timer(0.1, lambda: updater.update()) + + while len(self.received_status) < 1: + rclpy.spin_once(updater_node) + rclpy.spin_once(self.node) + + self.assertEqual(self.received_status[0].name, self.updater_node_name + ': test check') + self.assertEqual(self.received_status[0].message, 'test') + self.assertEqual(self.received_status[0].level, DiagnosticStatus.OK) + self.assertEqual(self.received_status[0].hardware_id, 'hardware_test') + updater_node.destroy_node() + + def test_fully_qualified_name(self): + param = [rclpy.Parameter('diagnostic_updater.use_fqn', rclpy.Parameter.Type.BOOL, True)] + updater_node = rclpy.create_node(node_name=self.updater_node_name, + namespace='test_namespace', parameter_overrides=param, + automatically_declare_parameters_from_overrides=True) + updater = Updater(updater_node) + updater.setHardwareID('hardware_test') + updater.add('test_check', self.update_diagnostics) + updater_node.create_timer(0.1, lambda: updater.update()) + + while len(self.received_status) < 1: + rclpy.spin_once(updater_node) + rclpy.spin_once(self.node) + + self.assertEqual(self.received_status[0].name, + '/test_namespace/updater_node: test_check') + self.assertEqual(self.received_status[0].message, 'test') + self.assertEqual(self.received_status[0].level, DiagnosticStatus.OK) + self.assertEqual(self.received_status[0].hardware_id, 'hardware_test')