diff --git a/msg/Metric.msg b/msg/Metric.msg new file mode 100644 index 0000000000000000000000000000000000000000..c583db848b824496962a44911f2bf9e1e942633e --- /dev/null +++ b/msg/Metric.msg @@ -0,0 +1,4 @@ +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 diff --git a/msg/Monitoring.msg b/msg/Monitoring.msg index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0c0d06e9e47b4a401532b46518420f82dd664501 100644 --- a/msg/Monitoring.msg +++ b/msg/Monitoring.msg @@ -0,0 +1,3 @@ +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 diff --git a/scripts/monitor.py b/scripts/monitor.py index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..2e396d2e3e95a3a4bff181a60221e30d190ca8ce 100755 --- a/scripts/monitor.py +++ b/scripts/monitor.py @@ -0,0 +1,98 @@ +#!/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() +