NTP monitor improvements (#342) (#351)

* NTP monitor improvements
* commit to run tests again
* bring back double callback
* remove whiteline

---------

Co-authored-by: Angsa Deployment Team <[email protected]>
(cherry picked from commit 7efb71a)

Co-authored-by: Tony Najjar <[email protected]>

diagnostic_common_diagnostics/diagnostic_common_diagnostics/ntp_monitor.py

@@ -50,6 +50,8 @@ def __init__(self, ntp_hostname, ntp_port, offset=500, self_offset=500,
                  do_self_test=True):
         """Initialize the NTPMonitor."""
         super().__init__(__class__.__name__)
+        self.declare_parameter('frequency', 10.0)
+        frequency = self.get_parameter('frequency').get_parameter_value().double_value
 
         self.ntp_hostname = ntp_hostname
         self.ntp_port = ntp_port
@@ -85,8 +87,8 @@ def __init__(self, ntp_hostname, ntp_port, offset=500, self_offset=500,
 
         # we need to periodically republish this
         self.current_msg = None
-        self.pubtimer = self.create_timer(0.1, self.pubCB)
-        self.checktimer = self.create_timer(0.1, self.checkCB)
+        self.pubtimer = self.create_timer(1/frequency, self.pubCB)
+        self.checktimer = self.create_timer(1/frequency, self.checkCB)
 
     def pubCB(self):
         with self.mutex:
@@ -95,6 +97,7 @@ def pubCB(self):
 
     def checkCB(self):
         new_msg = DIAG.DiagnosticArray()
+        new_msg.header.stamp = self.get_clock().now().to_msg()
 
         st = self.ntp_diag(self.stat)
         if st is not None:
@@ -159,7 +162,7 @@ def add_kv(stat_values, key, value):
 
 def ntp_monitor_main(argv=sys.argv[1:]):
     # filter out ROS args
-    argv = [a for a in argv if not a.startswith('__') and not a == '--ros-args' and not a == '-r']
+    argv = argv[:argv.index('--ros-args')] if '--ros-args' in argv else argv
 
     import argparse
     parser = argparse.ArgumentParser()