feat: add param to use fqn in updater (#320)

* feat: add param to use fqn in updater
---------
Co-authored-by: Christian Henkel <christian.henkel2@de.bosch.com>

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
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+  )
+  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})

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

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<bool>() ?
+      base_interface->get_fully_qualified_name() : base_interface->get_name();
   }
 
   /**

diagnostic_updater/package.xml

@@ -32,6 +32,7 @@
   <test_depend>ament_lint_common</test_depend>
   <test_depend>launch</test_depend>
   <test_depend>launch_testing</test_depend>
+  <!-- <test_depend>launch_testing_ros</test_depend> -->
   <test_depend>python3-pytest</test_depend>
   <test_depend>rclcpp_lifecycle</test_depend>
 

diagnostic_updater/test/status_msg_test.cpp

@@ -0,0 +1,142 @@
+/*********************************************************************
+* 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 <gtest/gtest.h>
+
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+
+#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<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostic_array_publisher_;
+  diagnostic_updater::Updater updater_;
+};
+
+class ListenerNode : public rclcpp::Node
+{
+public:
+  ListenerNode()
+  : Node("listener_node")
+  {
+    subscriber_ =
+      this->create_subscription<diagnostic_msgs::msg::DiagnosticArray>(
+      "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<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr
+    subscriber_;
+};
+
+TEST(TestStatusMsg, test_name) {
+  auto mock_node = std::make_shared<MockUpdaterNode>();
+  auto listener_node = std::make_shared<ListenerNode>();
+
+  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<MockUpdaterNode>(options);
+  auto listener_node = std::make_shared<ListenerNode>();
+
+  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;
+}

diagnostic_updater/test/status_msg_test.py

@@ -0,0 +1,73 @@
+import unittest
+
+from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
+from diagnostic_updater import Updater
+import rclpy
+
+
+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')