Observers

README.md

@@ -1,2 +1,28 @@
 # rosgraph_monitor
 
+## Installation
+```
+$ cd <path/to/workspace/src> git clone -b observers https://github.com/ipa-hsd/rosgraph_monitor/
+$ cd <path/to/workspace/src> git clone -b SoSymPaper https://github.com/ipa-nhg/ros_graph_parser
+$ cd <path/to/workspace>
+$ source /opt/ros/melodic/setup.bash
+$ rosdep install --from-paths src --ignore-src -r -y
+$ catkin build 
+$ source <path/to/workspace/devel/>setup.bash
+```
+
+## Running the system  
+source the workspace in all the terminals
+
+```
+# Terminal 1
+$ roscore
+
+# Terminal 2
+$ rosrun rosgraph_monitor monitor
+
+# Publish the topics listed in the `QualityObserver`
+
+# In a new terminal 
+$ rosservice call /load_observer "name: 'QualityObserver'"
+```

package.xml

@@ -4,13 +4,16 @@
   <version>0.0.1</version>
   <description>ROS graph monitor</description>
 
+  <author email="hsd@ipa.fraunhofer.de">Harsh Deshpande</author>
   <maintainer email="hsd@ipa.fraunhofer.de">Harsh Deshpande</maintainer>
   <license>Apache 2.0</license>
-  <author email="hsd@ipa.fraunhofer.de">Harsh Deshpande</author>
 
   <buildtool_depend>catkin</buildtool_depend>
 
   <exec_depend>rospy</exec_depend>
+  <exec_depend>controller_manager_msgs</exec_depend>
+  <exec_depend>diagnostic_msgs</exec_depend>
+  <exec_depend>ros_graph_parser</exec_depend>
 
   <export>
   </export>

scripts/monitor

@@ -1,137 +1,93 @@
 #!/usr/bin/env python
 
+import importlib
+import time
+import inspect
+import pkgutil
+
 import rospy
-from rosgraph_monitor.monitor_manager import MonitorManager, ServiceWrapper
-from rosgraph_monitor.parser import ModelParser
-from pyparsing import *
-import os.path
-import re
-
-from ros_graph_parser.srv import GetROSModel, GetROSSystemModel
-from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
-
-
-def strip_slash(string):
-    return '{}'.format(string[1:] if string.startswith('/') else string)
-
-
-class GraphScanService(ServiceWrapper):
-    def __init__(self, haros_model):
-        super(GraphScanService, self).__init__(
-            'get_rossystem_model', GetROSSystemModel)
-        self._rossystem_parser = ModelParser(haros_model)
-
-    # This function needs to be implemented by every service wrapper
-    # extract diagnostics from response here
-    def diagnostics_from_response(self, resp):
-        parser = ModelParser(resp.model, isFile=False)
-        dynamic_model = parser.parse()
-        static_model = self._rossystem_parser.parse()
-
-        missing_interfaces, additional_interfaces, incorrect_params = self.compare_models(
-            static_model, dynamic_model)
-
-        status_msgs = list()
-        if (not missing_interfaces) & (not additional_interfaces) & (not incorrect_params):
-            status_msg = DiagnosticStatus()
-            status_msg.level = DiagnosticStatus.OK
-            status_msg.name = "ROS Graph"
-            status_msg.message = "running OK"
-            status_msgs.append(status_msg)
-
-        else:
-            # Here are 2 'for loops' - 1 for missing and 1 for additional
-            for interface in missing_interfaces:
-                status_msg = DiagnosticStatus()
-                status_msg.level = DiagnosticStatus.ERROR
-                status_msg.name = interface
-                status_msg.message = "Missing node"
-                status_msgs.append(status_msg)
-
-            for interface in additional_interfaces:
-                status_msg = DiagnosticStatus()
-                status_msg.level = DiagnosticStatus.ERROR
-                status_msg.name = interface
-                status_msg.message = "Additional node"
-                status_msgs.append(status_msg)
-
-            for interface in incorrect_params:
-                status_msg = DiagnosticStatus()
-                status_msg.level = DiagnosticStatus.ERROR
-                status_msg.name = interface
-                status_msg.message = "Wrong param configuration"
-                for params in incorrect_params[interface]:
-                    status_msg.values.append(
-                        KeyValue(params[0], str(params[1])))
-                status_msgs.append(status_msg)
-
-        print(status_msg)
-        return status_msgs
-
-    # find out missing and additional interfaces
-    # if both lists are empty, system is running fine
-    def compare_models(self, model_ref, model_current):
-        # not sure of the performance of this method
-        set_ref = set((strip_slash(x.interface_name[0]))
-                      for x in model_ref.interfaces)
-        set_current = set((strip_slash(x.interface_name[0]))
-                          for x in model_current.interfaces)
-
-        # similarly for all interfaces within the node?
-        # or only for topic connections?
-        # does LED's code capture topic connections?
-        ref_params = dict()
-        for interface in model_ref.interfaces:
-            for param in interface.parameters:
-                key = strip_slash(param.param_name[0])
-                ref_params[key] = [param.param_value[0],
-                                   interface.interface_name[0]]
-
-        current_params = dict()
-        for interface in model_current.interfaces:
-            for param in interface.parameters:
-                key = strip_slash(param.param_name[0])
-                current_params[key] = [
-                    param.param_value[0], interface.interface_name[0]]
-
-        incorrect_params = dict()
-        for key, value in ref_params.items():
-            try:
-                current_value = current_params[key][0]
-                ref_value = ref_params[key][0]
-
-                if (type(current_value) is ParseResults) & (type(ref_value) is ParseResults):
-                    current_value = current_value.asList()
-                    ref_value = ref_value.asList()
-                if (type(current_value) is str) & (type(ref_value) is str):
-                    current_value = re.sub(
-                        r"[\n\t\s]*", "", strip_slash(current_value))
-                    ref_value = re.sub(
-                        r"[\n\t\s]*", "", strip_slash(ref_value))
-                isEqual = current_value == ref_value
-                if not isEqual:
-                    incorrect_params.setdefault(current_params[key][1], [])
-                    incorrect_params[current_params[key]
-                                     [1]].append([key, current_value])
-            except Exception as exc:
-                pass
-
-        # returning missing_interfaces, additional_interfaces
-        return list(set_ref - set_current), list(set_current - set_ref), incorrect_params
+import rosgraph_monitor.observers
+from controller_manager_msgs.srv import *
+
+
+def iter_namespace(ns_pkg):
+    return pkgutil.iter_modules(ns_pkg.__path__, ns_pkg.__name__ + ".")
+
+
+class ModuleManager(object):
+    def __init__(self):
+        self._modules = {}
+        self._observers = {}
+        rospy.Service('/load_observer', LoadController, self.handle_load)
+        rospy.Service('/unload_observer', UnloadController, self.handle_unload)
+        rospy.Service('/active_observers',
+                      ListControllerTypes, self.handle_active)
+        rospy.Service('/list_observers',
+                      ListControllerTypes, self.handle_types)
+
+    def handle_load(self, req):
+        started = self.start_observer(req.name)
+        return LoadControllerResponse(started)
+
+    def handle_unload(self, req):
+        stopped = self.stop_observer(req.name)
+        return UnloadControllerResponse(stopped)
+
+    def handle_active(self, req):
+        names = self._observers.keys()
+        return ListControllerTypesResponse(names, [])
+
+    def handle_types(self, req):
+        names = self._modules.keys()
+        return ListControllerTypesResponse(names, [])
+
+    def load_observers(self):
+        available_plugins = {
+            name: importlib.import_module(name)
+            for finder, name, ispkg
+            in iter_namespace(rosgraph_monitor.observers)
+        }
+        self._modules = self._get_leaf_nodes(
+            rosgraph_monitor.observer.Observer)
+
+    def start_observer(self, name):
+        started = True
+        try:
+            module = self._modules[name]
+            self._observers[name] = getattr(module, name)(name)
+            self._observers[name].start()
+        except Exception as exc:
+            print("Could not start {}".format(name))
+            started = False
+        return started
+
+    def stop_observer(self, name):
+        stopped = True
+        try:
+            self._observers[name].stop()
+            del self._observers[name]
+        except Exception as exc:
+            print("Could not stop {}".format(name))
+            stopped = False
+        return stopped
+
+    def _get_leaf_nodes(self, root):
+        leafs = {}
+        self._collect_leaf_nodes(root, leafs)
+        return leafs
+
+    def _collect_leaf_nodes(self, node, leafs):
+        if node is not None:    # change this to see if it is class
+            if len(node.__subclasses__()) == 0:
+                leafs[node.__name__] = inspect.getmodule(node)
+            for n in node.__subclasses__():
+                self._collect_leaf_nodes(n, leafs)
 
 
 if __name__ == "__main__":
     rospy.init_node('graph_monitor')
-    manager = MonitorManager()
-
-    my_path = os.path.abspath(os.path.dirname(__file__))
-    path = os.path.join(
-        my_path, "../resources/cob4-25.rossystem")
 
-    # how can this be read from a YAML file
-    # ideally should have service name and type only
-    graph_service = GraphScanService(path)
-    manager.register_service(graph_service)
+    manager = ModuleManager()
+    manager.load_observers()
 
-    manager.loop()
     rospy.spin()