From 2291d0d3306d3253a5af277b74e3d9f2d4013569 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Apr 2020 00:13:35 +0200 Subject: [PATCH 1/8] modified observer framework added ServiceObserver and TopicObserver sub-classes --- scripts/monitor | 4 - src/rosgraph_monitor/monitor_manager.py | 71 -------------- src/rosgraph_monitor/observer.py | 117 ++++++++++++++++++++++++ 3 files changed, 117 insertions(+), 75 deletions(-) delete mode 100644 src/rosgraph_monitor/monitor_manager.py create mode 100644 src/rosgraph_monitor/observer.py diff --git a/scripts/monitor b/scripts/monitor index a261a49..45f091d 100755 --- a/scripts/monitor +++ b/scripts/monitor @@ -1,7 +1,6 @@ #!/usr/bin/env python import rospy -from rosgraph_monitor.monitor_manager import MonitorManager, ServiceWrapper from rosgraph_monitor.parser import ModelParser from pyparsing import * import os.path @@ -122,8 +121,6 @@ class GraphScanService(ServiceWrapper): 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") @@ -133,5 +130,4 @@ if __name__ == "__main__": graph_service = GraphScanService(path) manager.register_service(graph_service) - 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..f4f0b1e --- /dev/null +++ b/src/rosgraph_monitor/observer.py @@ -0,0 +1,117 @@ +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, param, topics): + self._param_val = rospy.get_param(param) + self._topics = topics + super(TopicObserver, self).__init__(name, loop_rate_hz) + + # Every derived class needs to override this + def perform_check(self, msgs): + # do calculations and check with param + return True, "ERROR" + + def generate_diagnostics(self): + msgs = [] + 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)) + isResult, error_message = self.perform_check(msgs) + + status_msgs = list() + status_msg = DiagnosticStatus() + status_msg.level = DiagnosticStatus.OK if isResult else DiagnosticStatus.ERROR + status_msg.name = self._name + status_msg.message = "running OK" if isResult else error_message + status_msgs.append(status_msg) + + return status_msgs From d2b99f62a0d0b6e3179591ba672d9b48769f87b4 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Apr 2020 00:20:25 +0200 Subject: [PATCH 2/8] ROSGraphObserver extends ServiceObserver --- scripts/monitor | 124 ------------------ src/rosgraph_monitor/observers/__init__.py | 0 .../observers/graph_observer.py | 122 +++++++++++++++++ 3 files changed, 122 insertions(+), 124 deletions(-) create mode 100644 src/rosgraph_monitor/observers/__init__.py create mode 100644 src/rosgraph_monitor/observers/graph_observer.py diff --git a/scripts/monitor b/scripts/monitor index 45f091d..5d3da08 100755 --- a/scripts/monitor +++ b/scripts/monitor @@ -1,133 +1,9 @@ #!/usr/bin/env python import rospy -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 if __name__ == "__main__": rospy.init_node('graph_monitor') - 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) rospy.spin() 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 From 8f35de26a8431a8a7e50a501f6c8af385b55f6e2 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Apr 2020 00:21:25 +0200 Subject: [PATCH 3/8] loads observers automatically --- scripts/monitor | 84 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 84 insertions(+) diff --git a/scripts/monitor b/scripts/monitor index 5d3da08..f8d13e5 100755 --- a/scripts/monitor +++ b/scripts/monitor @@ -1,9 +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() From b2b3e97f6e828c4e4d83f58776bbe188abdd1073 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Apr 2020 00:22:10 +0200 Subject: [PATCH 4/8] example impl of TopicObserver --- .../observers/quality_observer.py | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) create mode 100644 src/rosgraph_monitor/observers/quality_observer.py diff --git a/src/rosgraph_monitor/observers/quality_observer.py b/src/rosgraph_monitor/observers/quality_observer.py new file mode 100644 index 0000000..8a5dcb2 --- /dev/null +++ b/src/rosgraph_monitor/observers/quality_observer.py @@ -0,0 +1,23 @@ +from rosgraph_monitor.observer import TopicObserver +from std_msgs.msg import Int32 + + +class QualityObserver(TopicObserver): + def __init__(self, name): + param = "/quality" + topics = [("/speed", Int32), ("/accel", Int32)] # list of pairs + + super(QualityObserver, self).__init__( + name, 10, param, topics) + + def perform_check(self, msgs): + if len(msgs) < 2: + return False, "Incorrect number of messages" + if not isinstance(msgs[0], Int32) or not isinstance(msgs[1], Int32): + return False, "Incorrect instance of message" + + isLower = (msgs[0].data + msgs[1].data) < int(self._param_val) + print( + "{0} + {1} < {2}".format(msgs[0].data, msgs[1].data, self._param_val)) + + return isLower, "Higher than expected" # Error message From 6e986bfce4916773b0b7a0d1365fdcfeee1561f0 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 27 Apr 2020 00:22:43 +0200 Subject: [PATCH 5/8] [WIP] LogObserver impl --- src/rosgraph_monitor/observers/log_observer.py | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 src/rosgraph_monitor/observers/log_observer.py 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 From 817c460fbc09462e01d9ced4a2e072c69300a662 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Tue, 28 Apr 2020 11:07:21 +0200 Subject: [PATCH 6/8] calculate_attr in TopicObserver returns DiagnosticStatus msg --- src/rosgraph_monitor/observer.py | 16 +++++------ .../observers/quality_observer.py | 27 ++++++++++++------- 2 files changed, 24 insertions(+), 19 deletions(-) diff --git a/src/rosgraph_monitor/observer.py b/src/rosgraph_monitor/observer.py index f4f0b1e..00b8761 100644 --- a/src/rosgraph_monitor/observer.py +++ b/src/rosgraph_monitor/observer.py @@ -88,15 +88,15 @@ def diagnostics_from_response(self, response): class TopicObserver(Observer): - def __init__(self, name, loop_rate_hz, param, topics): - self._param_val = rospy.get_param(param) + def __init__(self, name, loop_rate_hz, topics): self._topics = topics + self._id = "" super(TopicObserver, self).__init__(name, loop_rate_hz) # Every derived class needs to override this - def perform_check(self, msgs): - # do calculations and check with param - return True, "ERROR" + def calculate_attr(self, msgs): + # do calculations + return DiagnosticStatus() def generate_diagnostics(self): msgs = [] @@ -105,13 +105,9 @@ def generate_diagnostics(self): msgs.append(rospy.wait_for_message(topic, topic_type)) except rospy.ROSException as exc: print("Topic {} is not found: ".format(topic) + str(exc)) - isResult, error_message = self.perform_check(msgs) + status_msg = self.calculate_attr(msgs) status_msgs = list() - status_msg = DiagnosticStatus() - status_msg.level = DiagnosticStatus.OK if isResult else DiagnosticStatus.ERROR - status_msg.name = self._name - status_msg.message = "running OK" if isResult else error_message status_msgs.append(status_msg) return status_msgs diff --git a/src/rosgraph_monitor/observers/quality_observer.py b/src/rosgraph_monitor/observers/quality_observer.py index 8a5dcb2..3033723 100644 --- a/src/rosgraph_monitor/observers/quality_observer.py +++ b/src/rosgraph_monitor/observers/quality_observer.py @@ -1,23 +1,32 @@ 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): - param = "/quality" topics = [("/speed", Int32), ("/accel", Int32)] # list of pairs super(QualityObserver, self).__init__( - name, 10, param, topics) + name, 10, topics) - def perform_check(self, msgs): + def calculate_attr(self, msgs): + status_msg = DiagnosticStatus() if len(msgs) < 2: - return False, "Incorrect number of messages" + print("Incorrect number of messages") + return status_msg if not isinstance(msgs[0], Int32) or not isinstance(msgs[1], Int32): - return False, "Incorrect instance of message" + print("Incorrect instance of message") + return status_msg - isLower = (msgs[0].data + msgs[1].data) < int(self._param_val) - print( - "{0} + {1} < {2}".format(msgs[0].data, msgs[1].data, self._param_val)) + attr = msgs[0].data + msgs[1].data + print("{0} + {1}".format(msgs[0].data, msgs[1].data)) - return isLower, "Higher than expected" # Error message + status_msg = DiagnosticStatus() + status_msg.level = DiagnosticStatus.OK + status_msg.name = self._id + status_msg.values.append( + KeyValue("enery", attr)) + status_msg.message = "QA status" + + return status_msg From 6bbe806cc983eff0985ff9c1b72b885e0da02a0f Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Wed, 29 Apr 2020 20:18:30 +0200 Subject: [PATCH 7/8] updated README --- README.md | 26 ++++++++++++++++++++++++++ package.xml | 5 ++++- 2 files changed, 30 insertions(+), 1 deletion(-) 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 From 87a415f0ca4da08bfd829ba84cf5fa567888deb2 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Thu, 30 Apr 2020 11:27:09 +0200 Subject: [PATCH 8/8] fixed msg checking for TopicObserver --- src/rosgraph_monitor/observer.py | 8 +++++++- src/rosgraph_monitor/observers/quality_observer.py | 8 +------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/rosgraph_monitor/observer.py b/src/rosgraph_monitor/observer.py index 00b8761..fc1fda9 100644 --- a/src/rosgraph_monitor/observer.py +++ b/src/rosgraph_monitor/observer.py @@ -91,6 +91,7 @@ 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 @@ -100,14 +101,19 @@ def calculate_attr(self, msgs): 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)) - status_msg = self.calculate_attr(msgs) + 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/quality_observer.py b/src/rosgraph_monitor/observers/quality_observer.py index 3033723..4c35275 100644 --- a/src/rosgraph_monitor/observers/quality_observer.py +++ b/src/rosgraph_monitor/observers/quality_observer.py @@ -12,12 +12,6 @@ def __init__(self, name): def calculate_attr(self, msgs): status_msg = DiagnosticStatus() - if len(msgs) < 2: - print("Incorrect number of messages") - return status_msg - if not isinstance(msgs[0], Int32) or not isinstance(msgs[1], Int32): - print("Incorrect instance of message") - return status_msg attr = msgs[0].data + msgs[1].data print("{0} + {1}".format(msgs[0].data, msgs[1].data)) @@ -26,7 +20,7 @@ def calculate_attr(self, msgs): status_msg.level = DiagnosticStatus.OK status_msg.name = self._id status_msg.values.append( - KeyValue("enery", attr)) + KeyValue("enery", str(attr))) status_msg.message = "QA status" return status_msg