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_;