Use timer

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})

diagnostic_updater/diagnostic_updater/_diagnostic_updater.py

@@ -222,47 +222,26 @@ 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 = ''
         self.warn_nohwid_done = False
 
     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."""

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)
 
 

diagnostic_updater/include/diagnostic_updater/DiagnosticStatusWrapper.hpp → 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 <stdarg.h>
 #include <cstdio>
@@ -295,4 +295,4 @@ DiagnosticStatusWrapper::addf(
   va_end(va);
 }
 }  // namespace diagnostic_updater
-#endif  // DIAGNOSTIC_UPDATER__DIAGNOSTICSTATUSWRAPPER_HPP_
+#endif  // DIAGNOSTIC_UPDATER__DIAGNOSTIC_STATUS_WRAPPER_HPP_