diff --git a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py index 80b5222ce..e05ba6de1 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 635ead06a..05d3bc71b 100644 --- a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp +++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp @@ -363,11 +363,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, + unsigned char starting_up_status = diagnostic_msgs::msg::DiagnosticStatus::OK) : Updater( node->get_node_base_interface(), node->get_node_clock_interface(), @@ -375,7 +378,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( @@ -385,7 +389,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, + unsigned char starting_up_status = diagnostic_msgs::msg::DiagnosticStatus::OK); /** * \brief Returns the interval between updates. @@ -478,6 +483,7 @@ class Updater : public DiagnosticTaskVector rclcpp::Publisher::SharedPtr publisher_; rclcpp::Logger logger_; + unsigned char starting_up_status_; std::string hwid_; std::string node_name_; bool warn_nohwid_done_; diff --git a/diagnostic_updater/src/diagnostic_updater.cpp b/diagnostic_updater/src/diagnostic_updater.cpp index bfd5ce6d9..188aa3113 100644 --- a/diagnostic_updater/src/diagnostic_updater.cpp +++ b/diagnostic_updater/src/diagnostic_updater.cpp @@ -44,7 +44,9 @@ Updater::Updater( std::shared_ptr logging_interface, std::shared_ptr parameters_interface, std::shared_ptr timers_interface, - std::shared_ptr topics_interface, double period) + std::shared_ptr topics_interface, + double period, + unsigned char starting_up_status) : verbose_(false), base_interface_(base_interface), timers_interface_(timers_interface), @@ -54,7 +56,8 @@ Updater::Updater( topics_interface, "/diagnostics", 1)), logger_(logging_interface->get_logger()), node_name_(base_interface->get_name()), - warn_nohwid_done_(false) + warn_nohwid_done_(false), + starting_up_status_(starting_up_status) { constexpr const char * period_param_name = "diagnostic_updater.period"; rclcpp::ParameterValue period_param; @@ -192,7 +195,7 @@ void Updater::addedTaskCallback(DiagnosticTaskInternal & task) { DiagnosticStatusWrapper stat; stat.name = task.getName(); - stat.summary(0, "Node starting up"); + stat.summary(starting_up_status_, "Node starting up"); publish(stat); } } // namespace diagnostic_updater