diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..5391d87 --- /dev/null +++ b/.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/ \ No newline at end of file diff --git a/src/rosgraph_monitor/monitor.py b/src/rosgraph_monitor/monitor.py new file mode 100755 index 0000000..11f480a --- /dev/null +++ b/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() diff --git a/src/rosgraph_monitor/observer.py b/src/rosgraph_monitor/observer.py index 9b39ccf..b473332 100644 --- a/src/rosgraph_monitor/observer.py +++ b/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) diff --git a/src/rosgraph_monitor/observers/quality_observer.py b/src/rosgraph_monitor/observers/quality_observer.py index 4c35275..b1a59d5 100644 --- a/src/rosgraph_monitor/observers/quality_observer.py +++ b/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()