From 732497b9f1ef2d3a30ddf5cc25a76d13c8a19913 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 25 Jun 2019 00:45:16 +0200 Subject: [PATCH 1/5] use ros2 timer instead of custom logic Signed-off-by: Karsten Knese replace custom timer logic with ros2 timer Signed-off-by: Karsten Knese rebase --- .../diagnostic_updater/_diagnostic_updater.py | 57 +++++----- .../diagnostic_updater/example.py | 7 +- .../diagnostic_updater/diagnostic_updater.hpp | 102 +++++++++--------- diagnostic_updater/src/example.cpp | 12 ++- 4 files changed, 87 insertions(+), 91 deletions(-) diff --git a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py index abcafb76b..7b156ef03 100644 --- a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py +++ b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py @@ -229,19 +229,15 @@ class Updater(DiagnosticTaskVector): reason. """ - 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 +245,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 +279,16 @@ 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 broadcast(self, lvl, msg): """ Output a message on all the known DiagnosticStatus. @@ -318,17 +310,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..f66fae0b2 100755 --- a/diagnostic_updater/diagnostic_updater/example.py +++ b/diagnostic_updater/diagnostic_updater/example.py @@ -228,14 +228,14 @@ def main(): # If we know that the state of the node just changed, we can force an # immediate update. - updater.force_update() + updater.update() # We can remove a task by refering to its name. 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/diagnostic_updater.hpp b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp index bb9d8ce83..92325ba6f 100644 --- a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp +++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp @@ -50,13 +50,8 @@ #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 { @@ -373,20 +368,28 @@ class Updater : public DiagnosticTaskVector * parameter. */ 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_(rclcpp::Duration(period * 1e9)), publisher_( rclcpp::create_publisher( topics_interface, "/diagnostics", 1)), @@ -394,10 +397,11 @@ 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(period, 0); + + reset_timer(); } /** @@ -406,28 +410,6 @@ class Updater : public DiagnosticTaskVector */ 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(); - } - - /** - * \brief Forces the diagnostics to update. - * - * Useful if the node has undergone a drastic state change that should be - * published immediately. - */ - void force_update() - { - update_diagnostic_period(); - next_time_ = rclcpp::Clock().now() + rclcpp::Duration(period_); - if (rclcpp::ok()) { bool warn_nohwid = hwid_.empty(); @@ -480,14 +462,23 @@ class Updater : public DiagnosticTaskVector * \brief Returns the interval between updates. */ - rcl_duration_value_t getPeriod() const {return period_;} + auto getPeriod() const {return period_;} + + /** + * \brief Sets the period as a rclcpp::Duration + */ + void setPeriod(rclcpp::Duration period) + { + period_ = period; + reset_timer(); + } /** * \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 +486,7 @@ class Updater : public DiagnosticTaskVector */ void setPeriod(double period) { - period_ = rcl_duration_value_t(period * 1e9); + setPeriod(rclcpp::Duration(period, 0)); } /** @@ -545,16 +536,27 @@ 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) */ - void update_diagnostic_period() - { - rcl_duration_value_t old_period = period_; - next_time_ = next_time_ + - rclcpp::Duration(period_ - old_period); // Update next_time_ - } + // 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 +598,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/src/example.cpp b/diagnostic_updater/src/example.cpp index 7f585fd3a..8fcabb693 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; /* @@ -223,7 +225,7 @@ int main(int argc, char ** argv) // If we know that the state of the node just changed, we can force an // immediate update. - updater.force_update(); + updater.update(); // We can remove a task by refering to its name. if (!updater.removeByName("Bound check")) { @@ -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; } From 532a233f94c104adca27a0d5e61a49b73433c57e Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 27 Aug 2019 13:21:32 -0700 Subject: [PATCH 2/5] maintain double precision for period --- .../diagnostic_updater/diagnostic_updater.hpp | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp index 92325ba6f..55f8c9948 100644 --- a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp +++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp @@ -250,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); @@ -264,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); @@ -303,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_); @@ -364,8 +361,8 @@ 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 */ template explicit Updater(NodeT node, double period = 1.0) @@ -389,7 +386,7 @@ class Updater : public DiagnosticTaskVector base_interface_(base_interface), timers_interface_(timers_interface), clock_(std::make_shared(RCL_ROS_TIME)), - period_(rclcpp::Duration(period * 1e9)), + period_(static_cast(period * 1e9)), publisher_( rclcpp::create_publisher( topics_interface, "/diagnostics", 1)), @@ -399,7 +396,7 @@ class Updater : public DiagnosticTaskVector { period = parameters_interface->declare_parameter( "diagnostic_updater.period", rclcpp::ParameterValue(period)).get(); - period_ = rclcpp::Duration(period, 0); + period_ = rclcpp::Duration(static_cast(period * 1e9)); reset_timer(); } @@ -461,7 +458,6 @@ class Updater : public DiagnosticTaskVector /** * \brief Returns the interval between updates. */ - auto getPeriod() const {return period_;} /** @@ -486,7 +482,7 @@ class Updater : public DiagnosticTaskVector */ void setPeriod(double period) { - setPeriod(rclcpp::Duration(period, 0)); + setPeriod(static_cast(period * 1e9)); } /** @@ -499,7 +495,6 @@ class Updater : public DiagnosticTaskVector * * \param msg Status message to output. */ - void broadcast(int lvl, const std::string msg) { std::vector status_vec; From c7efe7b7d667438502776feff6ae863f9f51cb79 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 28 Aug 2019 14:19:06 -0700 Subject: [PATCH 3/5] style and documentation touchups --- diagnostic_updater/CMakeLists.txt | 2 +- .../diagnostic_updater/_diagnostic_updater.py | 7 ++----- ...atusWrapper.hpp => diagnostic_status_wrapper.hpp} | 6 +++--- .../diagnostic_updater/diagnostic_updater.hpp | 12 +++++------- .../include/diagnostic_updater/publisher.hpp | 2 +- diagnostic_updater/test/diagnostic_updater_test.cpp | 3 ++- ...sWrapper.py => test_diagnostic_status_wrapper.py} | 0 7 files changed, 14 insertions(+), 18 deletions(-) rename diagnostic_updater/include/diagnostic_updater/{DiagnosticStatusWrapper.hpp => diagnostic_status_wrapper.hpp} (98%) rename diagnostic_updater/test/{test_DiagnosticStatusWrapper.py => test_diagnostic_status_wrapper.py} (100%) 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 7b156ef03..1bd4cbd39 100644 --- a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py +++ b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py @@ -222,11 +222,8 @@ 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 update function can always be triggered async to the period interval. """ def __init__(self, node, period=1.0): 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 55f8c9948..78f0375bb 100644 --- a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp +++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp @@ -46,7 +46,7 @@ #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" @@ -346,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 update function can always be triggered async to the period interval. */ class Updater : public DiagnosticTaskVector { @@ -363,6 +359,8 @@ class Updater : public DiagnosticTaskVector * * \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, double period = 1.0) 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/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 From 18f5c771a1ba8cd1b9374583cc0e28f09f6c2c70 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 2 Sep 2019 11:29:44 -0700 Subject: [PATCH 4/5] re-introduce force_update --- .../diagnostic_updater/_diagnostic_updater.py | 8 +- .../diagnostic_updater/example.py | 2 +- .../diagnostic_updater/diagnostic_updater.hpp | 118 ++++++++++-------- diagnostic_updater/src/example.cpp | 2 +- 4 files changed, 72 insertions(+), 58 deletions(-) diff --git a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py index 1bd4cbd39..738501d66 100644 --- a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py +++ b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py @@ -223,7 +223,7 @@ class Updater(DiagnosticTaskVector): 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_updater.period" ros2 parameter. - The update function can always be triggered async to the period interval. + The force_update function can always be triggered async to the period interval. """ def __init__(self, node, period=1.0): @@ -286,6 +286,12 @@ def period(self, period): self.timer.reset() self.timer = self.node.creat_timer(self.__period, self.udpate) + def force_update(self): + """ + Forces to send out an update for all known DiagnosticStatus. + """ + self.update() + def broadcast(self, lvl, msg): """ Output a message on all the known DiagnosticStatus. diff --git a/diagnostic_updater/diagnostic_updater/example.py b/diagnostic_updater/diagnostic_updater/example.py index f66fae0b2..675428187 100755 --- a/diagnostic_updater/diagnostic_updater/example.py +++ b/diagnostic_updater/diagnostic_updater/example.py @@ -228,7 +228,7 @@ def main(): # If we know that the state of the node just changed, we can force an # immediate update. - updater.update() + updater.force_update() # We can remove a task by refering to its name. if not updater.removeByName('Bound check'): diff --git a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp index 78f0375bb..d4b15b441 100644 --- a/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp +++ b/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.hpp @@ -347,7 +347,7 @@ class DiagnosticTaskVector * 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_updater.period" ros2 parameter. - * The update function can always be triggered async to the period interval. + * The force_update function can always be triggered async to the period interval. */ class Updater : public DiagnosticTaskVector { @@ -399,60 +399,6 @@ class Updater : public DiagnosticTaskVector reset_timer(); } - /** - * \brief Causes the diagnostics to update if the inter-update interval - * has been exceeded. - */ - void update() - { - 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); - } - } - /** * \brief Returns the interval between updates. */ @@ -483,6 +429,14 @@ class Updater : public DiagnosticTaskVector setPeriod(static_cast(period * 1e9)); } + /** + * \brief Forces to send out an update for all known DiagnosticStatus. + */ + void force_update() + { + update(); + } + /** * \brief Output a message on all the known DiagnosticStatus. * @@ -539,6 +493,60 @@ class Updater : public DiagnosticTaskVector std::bind(&Updater::update, this)); } + /** + * \brief Causes the diagnostics to update if the inter-update interval + * has been exceeded. + */ + void update() + { + 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) */ diff --git a/diagnostic_updater/src/example.cpp b/diagnostic_updater/src/example.cpp index 8fcabb693..a5ae3f823 100644 --- a/diagnostic_updater/src/example.cpp +++ b/diagnostic_updater/src/example.cpp @@ -225,7 +225,7 @@ int main(int argc, char ** argv) // If we know that the state of the node just changed, we can force an // immediate update. - updater.update(); + updater.force_update(); // We can remove a task by refering to its name. if (!updater.removeByName("Bound check")) { From 15dba7f8683383b716d09fb07fefbf5087142c16 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 3 Sep 2019 08:59:19 -0700 Subject: [PATCH 5/5] fix flake8 --- diagnostic_updater/diagnostic_updater/_diagnostic_updater.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py index 738501d66..70190fe4a 100644 --- a/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py +++ b/diagnostic_updater/diagnostic_updater/_diagnostic_updater.py @@ -287,9 +287,7 @@ def period(self, period): self.timer = self.node.creat_timer(self.__period, self.udpate) def force_update(self): - """ - Forces to send out an update for all known DiagnosticStatus. - """ + """Force sending out an update for all known DiagnosticStatus.""" self.update() def broadcast(self, lvl, msg):