adapt to calculator

.gitignore

@@ -0,0 +1,138 @@
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+cover/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+.pybuilder/
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+#   For a library or package, you might want to ignore these files since the code is
+#   intended to run in multiple environments; otherwise, check them in:
+# .python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# pytype static type analyzer
+.pytype/
+
+# Cython debug symbols
+cython_debug/

src/rosgraph_monitor/monitor.py

@@ -0,0 +1,93 @@
+#!/usr/bin/env python
+
+import importlib
+import time
+import inspect
+import pkgutil
+
+import rospy
+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 = ModuleManager()
+    manager.load_observers()
+
+    rospy.spin()

src/rosgraph_monitor/observer.py

@@ -5,7 +5,7 @@
 
 
 class Observer(object):
-    def __init__(self, name, loop_rate_hz=1):
+    def __init__(self, name, output_topic_name=None, msg_type=None, loop_rate_hz=1):
         self._name = name
         self._rate = rospy.Rate(loop_rate_hz)
         self._seq = 1
@@ -15,9 +15,6 @@ def __init__(self, name, loop_rate_hz=1):
         self._thread.daemon = True
         self._stop_event = threading.Event()
 
-        self._pub_diag = rospy.Publisher(
-            '/diagnostics', DiagnosticArray, queue_size=10)
-
     def __del__(self):
         if Observer:
             print("{} stopped".format(self._name))
@@ -40,6 +37,9 @@ def _run(self):
             self._rate.sleep()
 
     def start(self):
+        self._pub_diag = rospy.Publisher(
+            '/diagnostics', DiagnosticArray, queue_size=10
+        )
         print("starting {}...".format(self._name))
         self._thread.start()
 
@@ -92,8 +92,8 @@ def diagnostics_from_response(self, response):
 
 
 class TopicObserver(Observer):
-    def __init__(self, name, loop_rate_hz, topics):
-        super(TopicObserver, self).__init__(name, loop_rate_hz)
+    def __init__(self, name, output_topic_name, msg_type, loop_rate_hz, topics):
+        super(TopicObserver, self).__init__(name, output_topic_name, msg_type, loop_rate_hz)
         self._topics = topics
         self._id = ""
         self._num_topics = len(topics)

src/rosgraph_monitor/observers/quality_observer.py

@@ -8,7 +8,7 @@ def __init__(self, name):
         topics = [("/speed", Int32), ("/accel", Int32)]     # list of pairs
 
         super(QualityObserver, self).__init__(
-            name, 10, topics)
+            name=name, loop_rate_hz=10, topics=topics, output_topic_name=None, msg_type=None)
 
     def calculate_attr(self, msgs):
         status_msg = DiagnosticStatus()