added monitor class

msg/Metric.msg

+string  key  # Label of the value.
+string  val  # Value to track.
+string  unit # Unit of the value.
+float32 err  # Error level of the value.
\ No newline at end of file

msg/Monitoring.msg

+string name   # Name and origin of the monitor message.
+string desc   # Text describing the monitor message.
+Metric metric # Values to monitor.
\ No newline at end of file

scripts/monitor.py

+#!/usr/bin/env python
+import socket
+import rospy
+from monitoring.msg import Monitoring
+from monitoring.msg import Metic
+
+
+class Monitor:
+    def __init__(self, desc, freq=1, auto=True):
+        self.host = socket.gethostname()
+        self.node = rospy.get_name()
+        self.name = self.host + self.node
+        self.desc = desc
+
+        if not freq > 0:
+            print("Frequency must be greater than 0!")
+            quit()
+
+        if auto:
+            self.timer = rospy.Timer(rospy.Duration(1.0 / freq), self.pub_monitoring)
+
+        self.log = {}
+        self.pub = rospy.Publisher('monitoring', Monitoring, queue_size=1)
+        self.pub_times = 0
+
+        self.monitoring = Monitoring()
+        self.monitoring.name = self.name
+        self.monitoring.desc = self.desc
+
+    def rst_log_key(self, key):
+        self.log[key] = {'num': 0, 'val': 0, 'sum': 0, 'dur': rospy.get_rostime()}
+
+    def add_log_val(self, key, val, unit, err, mode=2):
+        if " " in key:  # A key cannot contain whitespace!
+            rospy.logwarn("[%s] whitespaces are not allowed in monitoring keys!", self.node)
+        else:
+            if key in self.log:
+                # Mode 1: Write the first obtained value into the
+                #         published message.
+                if mode == 1 and self.pub_times == 0:
+                    self.set_metric(key, val, unit, err)
+                    self.pub_times = 1
+
+                # Mode 2: Write the last obtained value into the
+                #         published message.
+                elif mode == 2:
+                    self.set_metric(key, val, unit, err)
+
+                # Mode 3: Remember and write the minimum obtained
+                #         value, after mode selection, into the
+                #         published message.
+                elif mode == 3:
+                    if val < self.log[key]['val']:
+                        self.log[key]['val'] = val
+
+                    self.set_metric(key, self.log[key]['val'], unit, err)
+
+                # Mode 4: Remember and write the maximum obtained
+                #         value, after mode selection, into the
+                #         published message.
+                elif mode == 4:
+                    if val > self.log[key]['val']:
+                        self.log[key]['val'] = val
+
+                    self.set_metric(key, self.log[key]['val'], unit, err)
+
+                # Mode 5: Write the average value over five seconds
+                #         into the published message.
+                elif mode == 5:
+                    dur = rospy.get_rostime() - self.log[key]['dur']
+                    if dur < rospy.Duration(5):
+                        self.log[self.name][key]['num'] += 1
+                        self.log[self.name][key]['sum'] += val
+                    else:
+                        quotient = self.log[key]['sum'] / (self.log[key]['num'] + 0.001)
+                        self.set_metric(key, quotient, unit, err)
+                        self.rst_log_key(key)
+            else:
+                self.rst_log_key(key)  # Add key to the logging dictionary.
+                self.set_metric(key, val, unit, err)
+
+    def set_metric(self, key, val, unit, err):
+        metric = Metic()
+        metric.key = str(key)
+        metric.val = str(val)
+        metric.unit = str(unit)
+        metric.err = err
+        self.monitoring.metric = metric
+
+    def rst_monitoring(self):
+        self.monitoring = Monitoring()
+        self.monitoring.name = self.name
+        self.monitoring.desc = self.desc
+
+    def pub_monitoring(self):
+        self.pub.publish(self.monitoring)
+        self.rst_monitoring()
+