diff --git a/README.md b/README.md index 928436b..9c8333a 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,28 @@ # rosgraph_monitor +## Installation +``` +$ cd git clone -b observers https://github.com/ipa-hsd/rosgraph_monitor/ +$ cd git clone -b SoSymPaper https://github.com/ipa-nhg/ros_graph_parser +$ cd +$ source /opt/ros/melodic/setup.bash +$ rosdep install --from-paths src --ignore-src -r -y +$ catkin build +$ source 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'" +``` diff --git a/package.xml b/package.xml index 96e8a6c..1d71f19 100644 --- a/package.xml +++ b/package.xml @@ -4,13 +4,16 @@ 0.0.1 ROS graph monitor + Harsh Deshpande Harsh Deshpande Apache 2.0 - Harsh Deshpande catkin rospy + controller_manager_msgs + diagnostic_msgs + ros_graph_parser diff --git a/scripts/monitor b/scripts/monitor index a261a49..f8d13e5 100755 --- a/scripts/monitor +++ b/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() diff --git a/src/rosgraph_monitor/monitor_manager.py b/src/rosgraph_monitor/monitor_manager.py deleted file mode 100644 index f39924e..0000000 --- a/src/rosgraph_monitor/monitor_manager.py +++ /dev/null @@ -1,71 +0,0 @@ -#!/usr/bin/env python - -import threading -import mutex -import rospy -from diagnostic_msgs.msg import DiagnosticArray - - -class ServiceWrapper(object): - def __init__(self, service_name=None, service_type=None): - self.name = service_name - self.type = service_type - self.client = None - - def generate_diagnostics(self): - resp = self.client.call() # do I need a try catch here? - status_msg = self.diagnostics_from_response(resp) - return status_msg - - # Every derived class needs to override this - def diagnostics_from_response(self, response): - msg = DiagnosticArray() - return msg - - -class MonitorManager(object): - def __init__(self): - loop_rate_hz = 1 - rate = rospy.Rate(loop_rate_hz) - - self._pub_diag = rospy.Publisher( - 'diagnostics', DiagnosticArray, queue_size=10) - self._services = [] - self._ser_lock = threading.Lock() - self._thread = threading.Thread( - target=self.call_all, args=(rate,)) - self._thread.daemon = True - - # wrong service not caught properly - # ERROR (in case of wrong type): thread.error: release unlocked lock - def register_service(self, service): - try: - rospy.wait_for_service(service.name, timeout=1.0) - service.client = rospy.ServiceProxy(service.name, service.type) - self._ser_lock.acquire() - self._services.append(service) - print("Service '" + service.name + - "' added of type" + str(service.type)) - except rospy.ServiceException as exc: - print("Service did not process request: " + str(exc)) - finally: - self._ser_lock.release() - - def call_all(self, rate): - seq = 1 - while not rospy.is_shutdown(): - diag_msg = DiagnosticArray() - diag_msg.header.stamp = rospy.get_rostime() - - self._ser_lock.acquire() - for service in self._services: - status_msg = service.generate_diagnostics() - diag_msg.status.extend(status_msg) - - self._pub_diag.publish(diag_msg) - self._ser_lock.release() - seq += 1 - rate.sleep() - - def loop(self): - self._thread.start() diff --git a/src/rosgraph_monitor/observer.py b/src/rosgraph_monitor/observer.py new file mode 100644 index 0000000..fc1fda9 --- /dev/null +++ b/src/rosgraph_monitor/observer.py @@ -0,0 +1,119 @@ +import threading +import mutex +import rospy +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus + + +class Observer(object): + def __init__(self, name, loop_rate_hz=1): + self._name = name + self._rate = rospy.Rate(loop_rate_hz) + self._seq = 1 + self._lock = threading.Lock() + self._thread = threading.Thread( + target=self._run) + 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)) + + # Every derived class needs to override this + def generate_diagnostics(self): + msg = DiagnosticArray() + return msg + + def _run(self): + while not rospy.is_shutdown() and not self._stopped(): + diag_msg = DiagnosticArray() + diag_msg.header.stamp = rospy.get_rostime() + + status_msgs = self.generate_diagnostics() + diag_msg.status.extend(status_msgs) + self._pub_diag.publish(diag_msg) + + self._seq += 1 + self._rate.sleep() + + def start(self): + print("starting {}...".format(self._name)) + self._thread.start() + + def stop(self): + self._lock.acquire() + self._stop_event.set() + self._lock.release() + + def _stopped(self): + self._lock.acquire() + isSet = self._stop_event.isSet() + self._lock.release() + return isSet + + +class ServiceObserver(Observer): + def __init__(self, name, service_name=None, service_type=None, loop_rate_hz=1): + self.name = service_name + self.type = service_type + self.client = None + self.start_service() + super(ServiceObserver, self).__init__(name, loop_rate_hz) + + def start_service(self): + try: + rospy.wait_for_service(self.name, timeout=1.0) + self.client = rospy.ServiceProxy(self.name, self.type) + print("Service '" + self.name + + "' added of type" + str(self.type)) + except rospy.ServiceException as exc: + print("Service {} is not running: ".format(self.name) + str(exc)) + + def generate_diagnostics(self): + try: + resp = self.client.call() + except rospy.ServiceException as exc: + print("Service {} did not process request: ".format( + self.name) + str(exc)) + status_msg = self.diagnostics_from_response(resp) + return status_msg + + # Every derived class needs to override this + def diagnostics_from_response(self, response): + msg = DiagnosticArray() + return msg + + +class TopicObserver(Observer): + def __init__(self, name, loop_rate_hz, topics): + self._topics = topics + self._id = "" + self._num_topics = len(topics) + super(TopicObserver, self).__init__(name, loop_rate_hz) + + # Every derived class needs to override this + def calculate_attr(self, msgs): + # do calculations + return DiagnosticStatus() + + def generate_diagnostics(self): + msgs = [] + received_all = True + for topic, topic_type in self._topics: + try: + msgs.append(rospy.wait_for_message(topic, topic_type)) + except rospy.ROSException as exc: + print("Topic {} is not found: ".format(topic) + str(exc)) + received_all = False + break + + status_msgs = list() + status_msg = DiagnosticStatus() + if received_all: + status_msg = self.calculate_attr(msgs) + status_msgs.append(status_msg) + + return status_msgs diff --git a/src/rosgraph_monitor/observers/__init__.py b/src/rosgraph_monitor/observers/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/rosgraph_monitor/observers/graph_observer.py b/src/rosgraph_monitor/observers/graph_observer.py new file mode 100644 index 0000000..abc3807 --- /dev/null +++ b/src/rosgraph_monitor/observers/graph_observer.py @@ -0,0 +1,122 @@ +import imp +from rosgraph_monitor.observer import ServiceObserver +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 ROSGraphObserver(ServiceObserver): + def __init__(self, name): + super(ROSGraphObserver, self).__init__( + name, '/get_rossystem_model', GetROSSystemModel) + + # TODO: path to model shouldn't be hardcoded + self._rossystem_parser = ModelParser( + "src/rosgraph_monitor/resources/talker_listener.rossystem") + + def diagnostics_from_response(self, resp): + status_msgs = list() + if resp is None: + return status_msgs + + 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.WARN + 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) + + 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 diff --git a/src/rosgraph_monitor/observers/log_observer.py b/src/rosgraph_monitor/observers/log_observer.py new file mode 100644 index 0000000..7870b8f --- /dev/null +++ b/src/rosgraph_monitor/observers/log_observer.py @@ -0,0 +1,11 @@ +from rosgraph_monitor.observer import Observer +from diagnostic_msgs.msg import DiagnosticArray + + +class LogObserver(Observer): + def __init__(self, name): + super(LogObserver, self).__init__(name, 1) + + def generate_diagnostics(self): + msg = DiagnosticArray() + return msg diff --git a/src/rosgraph_monitor/observers/quality_observer.py b/src/rosgraph_monitor/observers/quality_observer.py new file mode 100644 index 0000000..4c35275 --- /dev/null +++ b/src/rosgraph_monitor/observers/quality_observer.py @@ -0,0 +1,26 @@ +from rosgraph_monitor.observer import TopicObserver +from std_msgs.msg import Int32 +from diagnostic_msgs.msg import DiagnosticStatus, KeyValue + + +class QualityObserver(TopicObserver): + def __init__(self, name): + topics = [("/speed", Int32), ("/accel", Int32)] # list of pairs + + super(QualityObserver, self).__init__( + name, 10, topics) + + def calculate_attr(self, msgs): + status_msg = DiagnosticStatus() + + attr = msgs[0].data + msgs[1].data + print("{0} + {1}".format(msgs[0].data, msgs[1].data)) + + status_msg = DiagnosticStatus() + status_msg.level = DiagnosticStatus.OK + status_msg.name = self._id + status_msg.values.append( + KeyValue("enery", str(attr))) + status_msg.message = "QA status" + + return status_msg