From 5a962ed40b49f5cc31ab007d1d4acc57adcf5851 Mon Sep 17 00:00:00 2001
From: redvinaa <redvinaa@gmail.com>
Date: Mon, 17 Jun 2024 13:33:48 +0200
Subject: [PATCH 1/2] Add starting_up_state parameter to Updater

---
 .../diagnostic_updater/_diagnostic_updater.py  |  8 ++++++--
 .../diagnostic_updater/diagnostic_updater.hpp  | 18 ++++++++++++++----
 2 files changed, 20 insertions(+), 6 deletions(-)

diff --git a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py
index dc776a701..b8f21883a 100644
--- a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py
+++ b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py
@@ -232,7 +232,7 @@ class Updater(DiagnosticTaskVector):
     interval.
     """
 
-    def __init__(self, node, period=1.0):
+    def __init__(self, node, period=1.0, starting_up_status=DiagnosticStatus.OK):
         """Construct an updater class."""
         DiagnosticTaskVector.__init__(self)
         self.node = node
@@ -242,6 +242,7 @@ def __init__(self, node, period=1.0):
         self.__period = self.node.declare_parameter(
             self.period_parameter, period).value
         self.timer = self.node.create_timer(self.__period, self.update)
+        self.__starting_up_status = starting_up_status
 
         self.verbose = False
         self.hwid = ''
@@ -377,7 +378,10 @@ def publish(self, msg):
 
     def addedTaskCallback(self, task):
         """Publish a task (called when added to the updater)."""
+        if self.__starting_up_status is None:
+            return
+
         stat = DiagnosticStatusWrapper()
         stat.name = task.name
-        stat.summary(DiagnosticStatus.OK, 'Node starting up')
+        stat.summary(self.__starting_up_status, 'Node starting up')
         self.publish(stat)
diff --git a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp
index a301ada59..dbd992347 100644
--- a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp
+++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp
@@ -359,11 +359,13 @@ class Updater : public DiagnosticTaskVector
    *
    * \param node Node pointer to set up diagnostics
    * \param period Value in seconds to set the update period
+   * \param starting_up_status Diagnostic status to send as first message
    * \note The given period value not being used if the `diagnostic_updater.period`
    * ros2 parameter was set previously.
    */
   template<class NodeT>
-  explicit Updater(NodeT node, double period = 1.0)
+  explicit Updater(NodeT node, double period = 1.0,
+    int starting_up_status = diagnostic_msgs::msg::DiagnosticStatus::OK)
   : Updater(
       node->get_node_base_interface(),
       node->get_node_clock_interface(),
@@ -371,7 +373,8 @@ class Updater : public DiagnosticTaskVector
       node->get_node_parameters_interface(),
       node->get_node_timers_interface(),
       node->get_node_topics_interface(),
-      period)
+      period,
+      starting_up_status)
   {}
 
   Updater(
@@ -381,7 +384,8 @@ class Updater : public DiagnosticTaskVector
     std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> parameters_interface,
     std::shared_ptr<rclcpp::node_interfaces::NodeTimersInterface> timers_interface,
     std::shared_ptr<rclcpp::node_interfaces::NodeTopicsInterface> topics_interface,
-    double period = 1.0)
+    double period = 1.0,
+    int starting_up_status = diagnostic_msgs::msg::DiagnosticStatus::OK)
   : verbose_(false),
     base_interface_(base_interface),
     timers_interface_(timers_interface),
@@ -391,6 +395,7 @@ class Updater : public DiagnosticTaskVector
       rclcpp::create_publisher<diagnostic_msgs::msg::DiagnosticArray>(
         topics_interface, "/diagnostics", 1)),
     logger_(logging_interface->get_logger()),
+    starting_up_status_(starting_up_status),
     node_name_(base_interface->get_name()),
     warn_nohwid_done_(false)
   {
@@ -604,9 +609,13 @@ class Updater : public DiagnosticTaskVector
    */
   virtual void addedTaskCallback(DiagnosticTaskInternal & task)
   {
+    if (starting_up_status_ < 0) {
+      return;
+    }
+
     DiagnosticStatusWrapper stat;
     stat.name = task.getName();
-    stat.summary(0, "Node starting up");
+    stat.summary(starting_up_status_, "Node starting up");
     publish(stat);
   }
 
@@ -618,6 +627,7 @@ class Updater : public DiagnosticTaskVector
   rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr publisher_;
   rclcpp::Logger logger_;
 
+  int starting_up_status_;
   std::string hwid_;
   std::string node_name_;
   bool warn_nohwid_done_;

From 692fd7cfb154e1c82ded6156ccbdb0554a075287 Mon Sep 17 00:00:00 2001
From: redvinaa <redvinaa@gmail.com>
Date: Tue, 18 Jun 2024 12:01:09 +0200
Subject: [PATCH 2/2] Lint

---
 .../include/diagnostic_updater/diagnostic_updater.hpp          | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp
index dbd992347..925171ca4 100644
--- a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp
+++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp
@@ -364,7 +364,8 @@ class Updater : public DiagnosticTaskVector
    * ros2 parameter was set previously.
    */
   template<class NodeT>
-  explicit Updater(NodeT node, double period = 1.0,
+  explicit Updater(
+    NodeT node, double period = 1.0,
     int starting_up_status = diagnostic_msgs::msg::DiagnosticStatus::OK)
   : Updater(
       node->get_node_base_interface(),