Add starting_up_state parameter to Updater

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)

diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp

@@ -359,19 +359,23 @@ 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(),
       node->get_node_logging_interface(),
       node->get_node_parameters_interface(),
       node->get_node_timers_interface(),
       node->get_node_topics_interface(),
-      period)
+      period,
+      starting_up_status)
   {}
 
   Updater(
@@ -381,7 +385,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 +396,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 +610,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 +628,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_;