diff --git a/CMakeLists.txt b/CMakeLists.txt
index fe7a707566c4d5b24a0cfbafe8b2346baa2beabe..7bd6a6af37b30ee1e022961eb316edcf6efb3d83 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -20,7 +20,7 @@ find_package(catkin REQUIRED COMPONENTS
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
+catkin_python_setup()
 
 ################################################
 ## Declare ROS messages, services and actions ##
@@ -49,6 +49,7 @@ find_package(catkin REQUIRED COMPONENTS
 ## Generate messages in the 'msg' folder
 add_message_files(
    FILES
+   Monitoring.msg
    Metric.msg
 )
 
@@ -159,15 +160,15 @@ include_directories(
 
 ## Mark executable scripts (Python etc.) for installation
 ## in contrast to setup.py, you can choose the destination
-# catkin_install_python(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
+catkin_install_python(PROGRAMS
+   nodes/monitor_test
+   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
 
 ## Mark executables for installation
 ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
 # install(TARGETS ${PROJECT_NAME}_node
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+#    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 # )
 
 ## Mark libraries for installation
@@ -187,9 +188,7 @@ include_directories(
 
 ## Mark other files for installation (e.g. launch and bag files, etc.)
 # install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+#    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 # )
 
 #############
diff --git a/launch/monitor_test.launch b/launch/monitor_test.launch
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/launch/watchdog.launch b/launch/watchdog.launch
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/msg/Metric.msg b/msg/Metric.msg
index 15c828ab5785563323de32f7ad3f9efd322470d1..26728b16295ff91c0abd7f28d1ce2721f36f0bad 100644
--- a/msg/Metric.msg
+++ b/msg/Metric.msg
@@ -1,4 +1,4 @@
-string  key      # Label of the metric.
+string  label    # Label of the metric.
 string  value    # Value of the metric.
 string  unit     # Unit of the metric.
 float32 critical # Critical level of the metric.
\ No newline at end of file
diff --git a/msg/Monitoring.msg b/msg/Monitoring.msg
index 4d9d229acf5f0a30e06115bcae2b66d808840315..4274d629d1d80b950e6489396e654c3e1be821dc 100644
--- a/msg/Monitoring.msg
+++ b/msg/Monitoring.msg
@@ -1,2 +1,3 @@
-string origin # Origin of the monitoring message.
+string family # Affiliation of the monitored metric.
+string origin # Origin of the monitored metric.
 Metric metric # Monitored metric.
\ No newline at end of file
diff --git a/nodes/monitor_test b/nodes/monitor_test
new file mode 100755
index 0000000000000000000000000000000000000000..01d669c6a008b9492c9d9abab4df56161604dcfe
--- /dev/null
+++ b/nodes/monitor_test
@@ -0,0 +1,21 @@
+#!/usr/bin/env python
+
+import rospy
+from monitoring.monitor import Monitor
+
+
+def monitor_test():
+    rospy.init_node("monitor_test")
+    monitor = Monitor(1, 1, "test")
+
+    rate = rospy.Rate(10)
+    while not rospy.is_shutdown():
+        monitor.update_metric("test", 10, "test", 0.5)
+        rate.sleep()
+
+
+if __name__ == '__main__':
+    try:
+        monitor_test()
+    except rospy.ROSInterruptException:
+        pass
diff --git a/nodes/watchdog b/nodes/watchdog
new file mode 100755
index 0000000000000000000000000000000000000000..24ef85bdc4a3b780ab1663b6b9567ca1f708e985
--- /dev/null
+++ b/nodes/watchdog
@@ -0,0 +1,21 @@
+#!/usr/bin/env python
+
+import rospy
+from monitoring.watchdog import Watchdog
+
+
+def watchdog():
+    rospy.init_node("watchdog")
+    watching = Watchdog(1.0)
+
+    rate = rospy.Rate(10)
+    while not rospy.is_shutdown():
+        print(watching.states)
+        rate.sleep()
+
+
+if __name__ == '__main__':
+    try:
+        watchdog()
+    except rospy.ROSInterruptException:
+        pass
diff --git a/scripts/watchdog.py b/scripts/watchdog.py
deleted file mode 100755
index 66758bda7cae21e8e48ae46dc7f13527080df452..0000000000000000000000000000000000000000
--- a/scripts/watchdog.py
+++ /dev/null
@@ -1,15 +0,0 @@
-#!/usr/bin/env python
-import rospy
-from monitoring.msg import Metric
-
-class Watchdog:
-    def __init__(self):
-        self.state = 0
-        self.observing = {}
-        rospy.init_node("watchdog")
-        rospy.Subscriber("metric", Metric, callback)
-
-    def callback(self, monitoring):
-
-        if monitoring.origin in self.observing:
-            self.observing[metric]
\ No newline at end of file
diff --git a/setup.py b/setup.py
new file mode 100644
index 0000000000000000000000000000000000000000..73f52d98fac0233fcc0ffc1fbab3a94731c44928
--- /dev/null
+++ b/setup.py
@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+setup_args = generate_distutils_setup(
+    packages=['monitoring'],
+    package_dir={'': 'src'}
+)
+
+setup(**setup_args)
diff --git a/src/monitoring/__init__.py b/src/monitoring/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/scripts/monitor.py b/src/monitoring/monitor.py
similarity index 62%
rename from scripts/monitor.py
rename to src/monitoring/monitor.py
index cb0120ca0f344ce19c4f6f72632a86f4e967be08..54c986a82822b97ea997a44dabd69509a5ed4ddd 100755
--- a/scripts/monitor.py
+++ b/src/monitoring/monitor.py
@@ -1,47 +1,50 @@
 #!/usr/bin/env python
-import socket
+
 import rospy
+import socket
 from monitoring.msg import Monitoring
 from monitoring.msg import Metric
 
 
 class Monitor:
-    def __init__(self, mode, freq):
+    def __init__(self, mode=1, freq=1.0, family="generic"):
         self.log = {}  # Logging dictionary. Is used to accomplish different publishing modes.
-        self.pub = rospy.Publisher('metric', Metric, queue_size=1)
+        self.pub = rospy.Publisher('monitoring', Monitoring, queue_size=1)
         self.mode = mode
-        self.monitoring = Monitoring()
-        self.monitoring.origin = socket.gethostname() + rospy.get_name()
 
-        if not freq > 0:
+        if not freq > 0.0:
             rospy.logwarn("Frequency must be greater then 0! Using 1 as frequency!")
-            freq = 1
+            freq = 1.0
 
         self.timer = rospy.Timer(rospy.Duration(1.0 / freq), self.publish_monitoring)
 
+        self.monitoring = Monitoring()
+        self.monitoring.family = family
+        self.monitoring.origin = socket.gethostname() + rospy.get_name()
+
     # At the moment the last metric update ist published over and over.
-    def publish_monitoring(self):
+    def publish_monitoring(self, event):
         self.pub.publish(self.monitoring)
 
-    def update_metric(self, key, value, unit, critical):
+    def update_metric(self, label, value, unit, critical):
         def set_metric():
             metric = Metric()
-            metric.key = key
-            metric.value = value
-            metric.unit = unit
+            metric.label = str(label)
+            metric.value = str(value)
+            metric.unit = str(unit)
             metric.critical = critical
 
             self.monitoring.metric = metric
 
         def rst_key():  # Reset the value of a key in the logging dictionary.
-            self.log[key] = {'num': 0, 'val': 0, 'sum': 0, 'dur': rospy.get_rostime()}
+            self.log[label] = {'num': 0, 'val': 0, 'sum': 0, 'dur': rospy.get_rostime()}
 
-        if " " in key:  # A key cannot contain whitespace!
-            rospy.logwarn("Whitespaces are not allowed in metric keys!")
+        if " " in label:  # A label cannot contain whitespace!
+            rospy.logwarn("Whitespaces are not allowed in metric labels!")
         elif critical > 1 or critical < 0:
             rospy.logwarn("Critical level must be between 1 and 0!")
         else:
-            if key in self.log:
+            if label in self.log:
                 # Mode 1: Set the last obtained value.
                 if self.mode == 1:
                     set_metric()
@@ -49,32 +52,32 @@ class Monitor:
                 # Mode 2: Remember and set the minimum value,
                 #         obtained after mode selection.
                 elif self.mode == 2:
-                    if value < self.log[key]['val']:
-                        self.log[key]['val'] = value
+                    if value < self.log[label]['val']:
+                        self.log[label]['val'] = value
                     else:
-                        value = self.log[key]['val']
+                        value = self.log[label]['val']
 
                     set_metric()
 
                 # Mode 3: Remember and set the maximum value,
                 #         obtained after mode selection.
                 elif self.mode == 3:
-                    if value > self.log[key]['val']:
-                        self.log[key]['val'] = value
+                    if value > self.log[label]['val']:
+                        self.log[label]['val'] = value
                     else:
-                        value = self.log[key]['val']
+                        value = self.log[label]['val']
 
                     set_metric()
 
                 # Mode 4: Set the average obtained value over five
                 #         or more seconds.
                 elif self.mode == 4:
-                    duration = rospy.get_rostime() - self.log[key]['dur']
+                    duration = rospy.get_rostime() - self.log[label]['dur']
                     if duration < rospy.Duration(5):
-                        self.log[key]['num'] += 1
-                        self.log[key]['sum'] += value
+                        self.log[label]['num'] += 1
+                        self.log[label]['sum'] += value
                     else:
-                        value = self.log[key]['sum'] / (self.log[key]['num'] + 0.001)
+                        value = self.log[label]['sum'] / (self.log[label]['num'] + 0.001)
                         set_metric()
                         rst_key()
             else:
diff --git a/src/monitoring/watchdog.py b/src/monitoring/watchdog.py
new file mode 100755
index 0000000000000000000000000000000000000000..829d0849a602d2b1a8d7253f1ae20f735e6a6fc3
--- /dev/null
+++ b/src/monitoring/watchdog.py
@@ -0,0 +1,55 @@
+#!/usr/bin/env python
+
+import rospy
+from monitoring.msg import Monitoring
+
+
+class Watchdog:
+    def __init__(self, freq):
+        self.states = {}
+        self.observing = {}
+
+        if not freq > 0.0:
+            rospy.logwarn("Frequency must be greater then 0! Using 1 as frequency!")
+            freq = 1.0
+
+        self.timer = rospy.Timer(rospy.Duration(1.0 / freq), self.update_state)
+
+        rospy.Subscriber("monitoring", Monitoring, self.logging)
+
+    def logging(self, monitoring):
+        family = monitoring.family
+        origin = monitoring.origin
+        metric = monitoring.metric
+
+        label = metric.label
+        value = metric.value
+        unit = metric.unit
+        critical = metric.critical
+
+        if family in self.observing:
+            if origin in self.observing[family]:
+                self.observing[family][origin].update({label: {"value": value, "unit": unit, "critical": critical}})
+            else:
+                self.observing[family][origin] = {}
+                self.observing[family][origin][label] = {"value": value, "unit": unit, "critical": critical}
+        else:
+            self.observing[family] = {}
+            self.observing[family][origin] = {}
+            self.observing[family][origin][label] = {"value": value, "unit": unit, "critical": critical}
+
+    def update_state(self, event):
+        for family in self.observing:
+            aggregation = 0
+            number = 0
+
+            for origin in self.observing[family]:
+                for label in self.observing[family][origin]:
+                    number = number + 1
+                    aggregation = aggregation + self.observing[family][origin][label]["critical"]
+
+            quotient = aggregation / number
+            self.states.update({family: quotient})
+
+
+