diff --git a/diagnostic_updater/CMakeLists.txt b/diagnostic_updater/CMakeLists.txt index 0e5c11ef8..d3cc91018 100644 --- a/diagnostic_updater/CMakeLists.txt +++ b/diagnostic_updater/CMakeLists.txt @@ -53,7 +53,7 @@ if(BUILD_TESTING) find_package(ament_cmake_pytest REQUIRED) ament_add_pytest_test(diagnostic_updater_test.py "test/diagnostic_updater_test.py") - ament_add_pytest_test(test_DiagnosticStatusWrapper.py "test/test_DiagnosticStatusWrapper.py") + ament_add_pytest_test(test_DiagnosticStatusWrapper.py "test/test_diagnostic_status_wrapper.py") endif() ament_python_install_package(${PROJECT_NAME}) diff --git a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py index abcafb76b..70190fe4a 100644 --- a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py +++ b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py @@ -222,26 +222,19 @@ class Updater(DiagnosticTaskVector): should be called frequently. At some predetermined rate, the update function will cause all the diagnostic tasks to run, and will collate and publish the resulting diagnostics. The publication rate is - determined by the "~diagnostic_period" ros parameter. - The class also allows an update to be forced when something significant - has happened, and allows a single message to be broadcast on all the - diagnostics if normal operation of the node is suspended for some - reason. + determined by the "~/diagnostic_updater.period" ros2 parameter. + The force_update function can always be triggered async to the period interval. """ - def __init__(self, node): + def __init__(self, node, period=1.0): """Construct an updater class.""" DiagnosticTaskVector.__init__(self) self.node = node self.publisher = self.node.create_publisher(DiagnosticArray, '/diagnostics', 1) self.clock = Clock() - now = self.clock.now() - - self.last_time = now - - self.last_time_period_checked = self.last_time self.period_parameter = 'diagnostic_updater.period' - self.period = self.node.declare_parameter(self.period_parameter, 1.0).value + self.__period = self.node.declare_parameter(self.period_parameter, period).value + self.timer = self.node.create_timer(self.__period, self.update) self.verbose = False self.hwid = '' @@ -249,20 +242,6 @@ def __init__(self, node): def update(self): """Causes the diagnostics to update if the inter-update interval has been exceeded.""" - self._check_diagnostic_period() - now = self.clock.now() - if now >= self.last_time: - self.force_update() - - def force_update(self): - """ - Force the diagnostics to update. - - Useful if the node has undergone a drastic state change that should be - published immediately. - """ - self.last_time = self.clock.now() - warn_nohwid = len(self.hwid) == 0 status_vec = [] @@ -297,6 +276,20 @@ def force_update(self): self.publish(status_vec) + @property + def period(self): + return self.__period + + @period.setter + def period(self, period): + self.__period = period + self.timer.reset() + self.timer = self.node.creat_timer(self.__period, self.udpate) + + def force_update(self): + """Force sending out an update for all known DiagnosticStatus.""" + self.update() + def broadcast(self, lvl, msg): """ Output a message on all the known DiagnosticStatus. @@ -318,17 +311,18 @@ def broadcast(self, lvl, msg): def setHardwareID(self, hwid): self.hwid = hwid - def _check_diagnostic_period(self): - """Recheck the diagnostic_period on the parameter server.""" - # This was getParamCached() call in the cpp code. i.e. it would throttle - # the actual call to the parameter server using a notification of change - # mechanism. - # This is not available in rospy. Hence I throttle the call to the - # parameter server using a standard timeout mechanism (4Hz) - now = self.clock.now() - if now >= self.last_time_period_checked: - self.period = self.node.get_parameter(self.period_parameter).value - self.last_time_period_checked = now + # TODO(Karsten1987) Re-enable this for eloquent + # def _check_diagnostic_period(self): + # """Recheck the diagnostic_period on the parameter server.""" + # # This was getParamCached() call in the cpp code. i.e. it would throttle + # # the actual call to the parameter server using a notification of change + # # mechanism. + # # This is not available in rospy. Hence I throttle the call to the + # # parameter server using a standard timeout mechanism (4Hz) + # now = self.clock.now() + # if now >= self.last_time_period_checked: + # # self.period = self.node.get_parameter(self.period_parameter).value + # self.last_time_period_checked = now def publish(self, msg): """Publish a single diagnostic status or a vector of diagnostic statuses.""" diff --git a/diagnostic_updater/diagnostic_updater/example.py b/diagnostic_updater/diagnostic_updater/example.py index 9dd09a94f..675428187 100755 --- a/diagnostic_updater/diagnostic_updater/example.py +++ b/diagnostic_updater/diagnostic_updater/example.py @@ -234,8 +234,8 @@ def main(): if not updater.removeByName('Bound check'): node.get_logger().error('The Bound check task was not found when trying to remove it.') + msg = std_msgs.msg.Bool() while rclpy.ok(): - msg = std_msgs.msg.Bool() sleep(0.1) # Calls to pub1 have to be accompanied by calls to pub1_freq to keep @@ -244,9 +244,6 @@ def main(): pub1.publish(msg) pub1_freq.tick() - # We can call updater.update whenever is convenient. It will take care - # of rate-limiting the updates. - updater.update() rclpy.spin_once(node, timeout_sec=1) diff --git a/diagnostic_updater/include/diagnostic_updater/DiagnosticStatusWrapper.hpp b/diagnostic_updater/include/diagnostic_updater/diagnostic_status_wrapper.hpp similarity index 98% rename from diagnostic_updater/include/diagnostic_updater/DiagnosticStatusWrapper.hpp rename to diagnostic_updater/include/diagnostic_updater/diagnostic_status_wrapper.hpp index 8ca371e67..1e884b114 100644 --- a/diagnostic_updater/include/diagnostic_updater/DiagnosticStatusWrapper.hpp +++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_status_wrapper.hpp @@ -36,8 +36,8 @@ * @author Blaise Gassend */ -#ifndef DIAGNOSTIC_UPDATER__DIAGNOSTICSTATUSWRAPPER_HPP_ -#define DIAGNOSTIC_UPDATER__DIAGNOSTICSTATUSWRAPPER_HPP_ +#ifndef DIAGNOSTIC_UPDATER__DIAGNOSTIC_STATUS_WRAPPER_HPP_ +#define DIAGNOSTIC_UPDATER__DIAGNOSTIC_STATUS_WRAPPER_HPP_ #include #include @@ -295,4 +295,4 @@ DiagnosticStatusWrapper::addf( va_end(va); } } // namespace diagnostic_updater -#endif // DIAGNOSTIC_UPDATER__DIAGNOSTICSTATUSWRAPPER_HPP_ +#endif // DIAGNOSTIC_UPDATER__DIAGNOSTIC_STATUS_WRAPPER_HPP_ diff --git a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp index bb9d8ce83..d4b15b441 100644 --- a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp +++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp @@ -46,17 +46,12 @@ #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "diagnostic_msgs/msg/diagnostic_status.hpp" -#include "diagnostic_updater/DiagnosticStatusWrapper.hpp" +#include "diagnostic_updater/diagnostic_status_wrapper.hpp" #include "rcl/time.h" -#include "rclcpp/clock.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/node.hpp" -#include "rclcpp/parameter_client.hpp" +#include "rclcpp/create_timer.hpp" #include "rclcpp/rclcpp.hpp" -#include "rclcpp/utilities.hpp" -#include "rclcpp/time.hpp" namespace diagnostic_updater { @@ -255,7 +250,6 @@ class DiagnosticTaskVector * called, and in particular it need not be valid at the time the * DiagnosticTaskVector is destructed. */ - void add(const std::string & name, TaskFunction f) { DiagnosticTaskInternal int_task(name, f); @@ -269,7 +263,6 @@ class DiagnosticTaskVector * least until the last time its diagnostic method is called. It need not be * valid at the time the DiagnosticTaskVector is destructed. */ - void add(DiagnosticTask & task) { TaskFunction f = std::bind(&DiagnosticTask::run, &task, std::placeholders::_1); @@ -308,7 +301,6 @@ class DiagnosticTaskVector * * \return Returns true if a task matched and was removed. */ - bool removeByName(const std::string name) { std::unique_lock lock(lock_); @@ -354,12 +346,8 @@ class DiagnosticTaskVector * should be called frequently. At some predetermined rate, the update * function will cause all the diagnostic tasks to run, and will collate * and publish the resulting diagnostics. The publication rate is - * determined by the "~diagnostic_period" ros parameter. - * - * The class also allows an update to be forced when something significant - * has happened, and allows a single message to be broadcast on all the - * diagnostics if normal operation of the node is suspended for some - * reason. + * determined by the "~/diagnostic_updater.period" ros2 parameter. + * The force_update function can always be triggered async to the period interval. */ class Updater : public DiagnosticTaskVector { @@ -369,24 +357,34 @@ class Updater : public DiagnosticTaskVector /** * \brief Constructs an updater class. * - * \param h Node handle from which to get the diagnostic_period - * parameter. + * \param node Node pointer to set up diagnostics + * \param period Value in seconds to set the update period + * \note The given period value not being used if the `diagnostic_updater.period` + * ros2 parameter was set previously. */ template - explicit Updater(NodeT node) + explicit Updater(NodeT node, double period = 1.0) : Updater( node->get_node_base_interface(), - node->get_node_topics_interface(), node->get_node_logging_interface(), - node->get_node_parameters_interface()) + node->get_node_parameters_interface(), + node->get_node_timers_interface(), + node->get_node_topics_interface(), + period) {} Updater( std::shared_ptr base_interface, - std::shared_ptr topics_interface, std::shared_ptr logging_interface, - std::shared_ptr parameters_interface) + std::shared_ptr parameters_interface, + std::shared_ptr timers_interface, + std::shared_ptr topics_interface, + double period = 1.0) : verbose_(false), + base_interface_(base_interface), + timers_interface_(timers_interface), + clock_(std::make_shared(RCL_ROS_TIME)), + period_(static_cast(period * 1e9)), publisher_( rclcpp::create_publisher( topics_interface, "/diagnostics", 1)), @@ -394,100 +392,33 @@ class Updater : public DiagnosticTaskVector node_name_(base_interface->get_name()), warn_nohwid_done_(false) { - double period = parameters_interface->declare_parameter( - "diagnostic_updater.period", rclcpp::ParameterValue(1.0)).get(); - period_ = static_cast(period * 1e9); - next_time_ = rclcpp::Clock().now() + rclcpp::Duration(period_); + period = parameters_interface->declare_parameter( + "diagnostic_updater.period", rclcpp::ParameterValue(period)).get(); + period_ = rclcpp::Duration(static_cast(period * 1e9)); + + reset_timer(); } /** - * \brief Causes the diagnostics to update if the inter-update interval - * has been exceeded. + * \brief Returns the interval between updates. */ - void update() - { - rclcpp::Time now_time = rclcpp::Clock().now(); - - if (now_time < next_time_) { - // @todo put this back in after fix of #2157 update_diagnostic_period(); - // Will be checked in force_update otherwise. - return; - } - - force_update(); - } + auto getPeriod() const {return period_;} /** - * \brief Forces the diagnostics to update. - * - * Useful if the node has undergone a drastic state change that should be - * published immediately. + * \brief Sets the period as a rclcpp::Duration */ - void force_update() + void setPeriod(rclcpp::Duration period) { - update_diagnostic_period(); - next_time_ = rclcpp::Clock().now() + rclcpp::Duration(period_); - - if (rclcpp::ok()) { - bool warn_nohwid = hwid_.empty(); - - std::vector status_vec; - - std::unique_lock lock( - lock_); // Make sure no adds happen while we are processing here. - const std::vector & tasks = getTasks(); - for (std::vector::const_iterator iter = - tasks.begin(); - iter != tasks.end(); iter++) - { - diagnostic_updater::DiagnosticStatusWrapper status; - - status.name = iter->getName(); - status.level = 2; - status.message = "No message was set"; - status.hardware_id = hwid_; - - iter->run(status); - - status_vec.push_back(status); - - if (status.level) { - warn_nohwid = false; - } - - if (verbose_ && status.level) { - RCLCPP_WARN( - logger_, "Non-zero diagnostic status. Name: '%s', status %i: '%s'", - status.name.c_str(), status.level, status.message.c_str()); - } - } - - if (warn_nohwid && !warn_nohwid_done_) { - std::string error_msg = "diagnostic_updater: No HW_ID was set."; - error_msg += " This is probably a bug. Please report it."; - error_msg += " For devices that do not have a HW_ID, set this value to 'none'."; - error_msg += " This warning only occurs once all diagnostics are OK."; - error_msg += " It is okay to wait until the device is open before calling setHardwareID."; - RCLCPP_WARN(logger_, error_msg); - warn_nohwid_done_ = true; - } - - publish(status_vec); - } + period_ = period; + reset_timer(); } - /** - * \brief Returns the interval between updates. - */ - - rcl_duration_value_t getPeriod() const {return period_;} - /** * \brief Sets the period as a rcl_duration_value_t */ void setPeriod(rcl_duration_value_t period) { - period_ = period; + setPeriod(rclcpp::Duration(period)); } /** @@ -495,7 +426,15 @@ class Updater : public DiagnosticTaskVector */ void setPeriod(double period) { - period_ = rcl_duration_value_t(period * 1e9); + setPeriod(static_cast(period * 1e9)); + } + + /** + * \brief Forces to send out an update for all known DiagnosticStatus. + */ + void force_update() + { + update(); } /** @@ -508,7 +447,6 @@ class Updater : public DiagnosticTaskVector * * \param msg Status message to output. */ - void broadcast(int lvl, const std::string msg) { std::vector status_vec; @@ -545,17 +483,82 @@ class Updater : public DiagnosticTaskVector void setHardwareID(const std::string & hwid) {hwid_ = hwid;} private: + void reset_timer() + { + update_timer_ = rclcpp::create_timer( + base_interface_.get(), + timers_interface_.get(), + clock_, + period_, + std::bind(&Updater::update, this)); + } + /** - * Recheck the diagnostic_period on the parameter server. (Cached) + * \brief Causes the diagnostics to update if the inter-update interval + * has been exceeded. */ - - void update_diagnostic_period() + void update() { - rcl_duration_value_t old_period = period_; - next_time_ = next_time_ + - rclcpp::Duration(period_ - old_period); // Update next_time_ + if (rclcpp::ok()) { + bool warn_nohwid = hwid_.empty(); + + std::vector status_vec; + + std::unique_lock lock( + lock_); // Make sure no adds happen while we are processing here. + const std::vector & tasks = getTasks(); + for (std::vector::const_iterator iter = + tasks.begin(); + iter != tasks.end(); iter++) + { + diagnostic_updater::DiagnosticStatusWrapper status; + + status.name = iter->getName(); + status.level = 2; + status.message = "No message was set"; + status.hardware_id = hwid_; + + iter->run(status); + + status_vec.push_back(status); + + if (status.level) { + warn_nohwid = false; + } + + if (verbose_ && status.level) { + RCLCPP_WARN( + logger_, "Non-zero diagnostic status. Name: '%s', status %i: '%s'", + status.name.c_str(), status.level, status.message.c_str()); + } + } + + if (warn_nohwid && !warn_nohwid_done_) { + std::string error_msg = "diagnostic_updater: No HW_ID was set."; + error_msg += " This is probably a bug. Please report it."; + error_msg += " For devices that do not have a HW_ID, set this value to 'none'."; + error_msg += " This warning only occurs once all diagnostics are OK."; + error_msg += " It is okay to wait until the device is open before calling setHardwareID."; + RCLCPP_WARN(logger_, error_msg); + warn_nohwid_done_ = true; + } + + publish(status_vec); + } } + /** + * Recheck the diagnostic_period on the parameter server. (Cached) + */ + + // TODO(Karsten1987) Follow up PR for eloquent + // void update_diagnostic_period() + // { + // // rcl_duration_value_t old_period = period_; + // // next_time_ = next_time_ + + // // rclcpp::Duration(period_ - old_period); // Update next_time_ + // } + /** * Publishes a single diagnostic status. */ @@ -596,12 +599,14 @@ class Updater : public DiagnosticTaskVector publish(stat); } + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr base_interface_; + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr timers_interface_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Duration period_; + rclcpp::TimerBase::SharedPtr update_timer_; rclcpp::Publisher::SharedPtr publisher_; rclcpp::Logger logger_; - rclcpp::Time next_time_; - - rcl_duration_value_t period_; std::string hwid_; std::string node_name_; bool warn_nohwid_done_; diff --git a/diagnostic_updater/include/diagnostic_updater/publisher.hpp b/diagnostic_updater/include/diagnostic_updater/publisher.hpp index 5c6febe48..72278c3f2 100644 --- a/diagnostic_updater/include/diagnostic_updater/publisher.hpp +++ b/diagnostic_updater/include/diagnostic_updater/publisher.hpp @@ -39,8 +39,8 @@ #include #include -#include "diagnostic_updater/update_functions.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "diagnostic_updater/update_functions.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp/subscription.hpp" diff --git a/diagnostic_updater/src/example.cpp b/diagnostic_updater/src/example.cpp index 7f585fd3a..a5ae3f823 100644 --- a/diagnostic_updater/src/example.cpp +++ b/diagnostic_updater/src/example.cpp @@ -37,6 +37,8 @@ #include +using namespace std::chrono_literals; + double time_to_launch; /* @@ -231,9 +233,9 @@ int main(int argc, char ** argv) node->get_logger(), "The Bound check task was not found when trying to remove it."); } + rclcpp::Rate r(500ms); while (rclcpp::ok()) { std_msgs::msg::Bool msg; - rclcpp::Rate(10).sleep(); // Calls to pub1 have to be accompanied by calls to pub1_freq to keep // the statistics up to date. @@ -241,11 +243,11 @@ int main(int argc, char ** argv) pub1->publish(msg); pub1_freq.tick(); - // We can call updater.update whenever is convenient. It will take care - // of rate-limiting the updates. - updater.update(); rclcpp::spin_some(node); + r.sleep(); } + rclcpp::shutdown(); + return 0; } diff --git a/diagnostic_updater/test/diagnostic_updater_test.cpp b/diagnostic_updater/test/diagnostic_updater_test.cpp index 88ff6421e..0362b2897 100644 --- a/diagnostic_updater/test/diagnostic_updater_test.cpp +++ b/diagnostic_updater/test/diagnostic_updater_test.cpp @@ -38,11 +38,12 @@ #include #include -#include "diagnostic_updater/DiagnosticStatusWrapper.hpp" +#include "diagnostic_updater/diagnostic_status_wrapper.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" #include "diagnostic_updater/update_functions.hpp" #include "rclcpp/rclcpp.hpp" + #include "rclcpp_lifecycle/lifecycle_node.hpp" using namespace std::chrono_literals; diff --git a/diagnostic_updater/test/test_DiagnosticStatusWrapper.py b/diagnostic_updater/test/test_diagnostic_status_wrapper.py similarity index 100% rename from diagnostic_updater/test/test_DiagnosticStatusWrapper.py rename to diagnostic_updater/test/test_diagnostic_status_wrapper.py