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..925171ca4 100644 --- a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp +++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp @@ -359,11 +359,14 @@ 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 - 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 +374,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 +385,8 @@ class Updater : public DiagnosticTaskVector std::shared_ptr parameters_interface, std::shared_ptr timers_interface, std::shared_ptr 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( 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::SharedPtr publisher_; rclcpp::Logger logger_; + int starting_up_status_; std::string hwid_; std::string node_name_; bool warn_nohwid_done_;