From af5b440289dc9b3c56a58a0219b25831235c9611 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Wed, 2 Sep 2020 15:16:46 +0200 Subject: [PATCH 1/8] Fix rospkg dep --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index bc08070..ab15d08 100644 --- a/package.xml +++ b/package.xml @@ -14,7 +14,7 @@ controller_manager_msgs diagnostic_msgs ros_graph_parser - rospkg + python-rospkg From 29fc2b5bd1bb2b54836aed3473830dde834c11ac Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Wed, 2 Sep 2020 15:17:26 +0200 Subject: [PATCH 2/8] Fix ros_graph_parser keywords --- src/rosgraph_monitor/parser.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/rosgraph_monitor/parser.py b/src/rosgraph_monitor/parser.py index 0cb245e..9161851 100644 --- a/src/rosgraph_monitor/parser.py +++ b/src/rosgraph_monitor/parser.py @@ -87,13 +87,13 @@ def __init__(self, model, isFile=True): _action_servers = Keyword("RosActionServers").suppress() _action_server = Keyword("RosActionServer").suppress( ) | Keyword("RosServer").suppress() - _ref_server = Keyword("RefServer").suppress() + _ref_server = Keyword("RefServer").suppress() | Keyword("RefActionServer").suppress() # Actio Clients Def _action_clients = Keyword("RosActionClients").suppress() _action_client = Keyword("RosActionClient").suppress( ) | Keyword("RosClient").suppress() - _ref_action_client = Keyword("RefClient").suppress() + _ref_action_client = Keyword("RefClient").suppress() | Keyword("RefActionClient").suppress() # Topic Connections Def _topic_connections = Keyword("TopicConnections").suppress() From 0f986faa226ffe467f23551b78404cefce80397e Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Wed, 2 Sep 2020 15:18:00 +0200 Subject: [PATCH 3/8] show more exception info need to improve more --- src/rosgraph_monitor/parser.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/rosgraph_monitor/parser.py b/src/rosgraph_monitor/parser.py index 9161851..514f081 100644 --- a/src/rosgraph_monitor/parser.py +++ b/src/rosgraph_monitor/parser.py @@ -196,8 +196,8 @@ def parse(self): self._parse_from_file() else: self._parse_from_string() - except Exception as e: - print(e.args) # Should set a default 'result'? + except ParseException as e: + print(ParseException.explain(e, depth=0)) # Should set a default 'result'? return self._result From 6d9ba050b43a0df8fafaaf60bc3ce22ece878103 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 30 Nov 2020 18:58:20 +0100 Subject: [PATCH 4/8] separate output interface (publisher) from observer --- scripts/monitor | 4 +- src/rosgraph_monitor/observer.py | 57 ++++++++++--------- .../observers/dummy_diag_observer.py | 29 ++++++++++ src/rosgraph_monitor/output_interface.py | 28 +++++++++ .../output_interfaces/__init__.py | 0 .../output_interfaces/diagnostic_publisher.py | 6 ++ 6 files changed, 94 insertions(+), 30 deletions(-) create mode 100644 src/rosgraph_monitor/observers/dummy_diag_observer.py create mode 100644 src/rosgraph_monitor/output_interface.py create mode 100644 src/rosgraph_monitor/output_interfaces/__init__.py create mode 100644 src/rosgraph_monitor/output_interfaces/diagnostic_publisher.py diff --git a/scripts/monitor b/scripts/monitor index f8d13e5..448ac23 100755 --- a/scripts/monitor +++ b/scripts/monitor @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import importlib import time @@ -55,7 +55,7 @@ class ModuleManager(object): try: module = self._modules[name] self._observers[name] = getattr(module, name)(name) - self._observers[name].start() + self._observers[name].start(self._observers[name].perform_output) except Exception as exc: print("Could not start {}".format(name)) started = False diff --git a/src/rosgraph_monitor/observer.py b/src/rosgraph_monitor/observer.py index 9b39ccf..a383d78 100644 --- a/src/rosgraph_monitor/observer.py +++ b/src/rosgraph_monitor/observer.py @@ -1,5 +1,4 @@ import threading -import mutex import rospy from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus @@ -8,39 +7,38 @@ 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 generate_diagnostics(self): + # msg = DiagnosticArray() + # return msg + def generate_output(): + return None def _run(self): + print("starting thread") while not rospy.is_shutdown() and not self._stopped(): - diag_msg = DiagnosticArray() - diag_msg.header.stamp = rospy.get_rostime() + msg = self.generate_output() + print(msg) - status_msgs = self.generate_diagnostics() - diag_msg.status.extend(status_msgs) - self._pub_diag.publish(diag_msg) + # perform_output() should be implemented by OutputInterface's sub-class + # what would be a better way of doing this? + self._perform_output(msg) - self._seq += 1 self._rate.sleep() - def start(self): + def start(self, func): print("starting {}...".format(self._name)) + self._perform_output = func self._thread.start() def stop(self): @@ -76,48 +74,51 @@ def start_service(self): except rospy.ServiceException as exc: print("Service {} is not running: ".format(self.name) + str(exc)) - def generate_diagnostics(self): + def generate_output(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 + # 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 + # def diagnostics_from_response(self, response): + # msg = DiagnosticArray() + # return msg class TopicObserver(Observer): def __init__(self, name, loop_rate_hz, topics): - super(TopicObserver, self).__init__(name, loop_rate_hz) + super(TopicObserver, self).__init__(name=name, loop_rate_hz=loop_rate_hz) self._topics = topics self._id = "" self._num_topics = len(topics) + def is_close(self, a, b, rel_tol=1e-09, abs_tol=0.0): + return abs(a - b) <= max(rel_tol * max(abs(a), abs(b)), abs_tol) + # Every derived class needs to override this def calculate_attr(self, msgs): # do calculations return DiagnosticStatus() - def generate_diagnostics(self): + def generate_output(self): msgs = [] received_all = True for topic, topic_type in self._topics: try: + # Add message synchronization here + # Consider "asynchronous" reception as well? 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() + msg = None if received_all: - status_msg = self.calculate_attr(msgs) - status_msgs.append(status_msg) + msg = self.calculate_attr(msgs) - return status_msgs + return msg diff --git a/src/rosgraph_monitor/observers/dummy_diag_observer.py b/src/rosgraph_monitor/observers/dummy_diag_observer.py new file mode 100644 index 0000000..82b8b36 --- /dev/null +++ b/src/rosgraph_monitor/observers/dummy_diag_observer.py @@ -0,0 +1,29 @@ +from rosgraph_monitor.observer import TopicObserver +from rosgraph_monitor.output_interfaces.diagnostic_publisher import DiagnosticPublisher +from std_msgs.msg import Int32 +from diagnostic_msgs.msg import DiagnosticStatus, KeyValue + + +class DummyDiagnosticObserver(TopicObserver, DiagnosticPublisher): + def __init__(self, name): #, set_point=None): # reads from param? + topics = [("/speed", Int32), ("/accel", Int32)] # list of pairs + self._set_point = 5.5 #set_point + DiagnosticPublisher.__init__(self, topic_name="/diagnostics") + TopicObserver.__init__(self, name=name, loop_rate_hz=10, topics=topics) + + def calculate_attr(self, msgs): + status_msg = DiagnosticStatus() + + attr = msgs[0].data + msgs[1].data + is_close = False + if(self._set_point): + is_close = self.is_close(attr, self._set_point) + print("{0} + {1}; is_close={2}".format(msgs[0].data, msgs[1].data, is_close)) + + 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 diff --git a/src/rosgraph_monitor/output_interface.py b/src/rosgraph_monitor/output_interface.py new file mode 100644 index 0000000..6aa5620 --- /dev/null +++ b/src/rosgraph_monitor/output_interface.py @@ -0,0 +1,28 @@ +import rospy + + +class OutputInterface(object): + def __init__(self): + pass + + def perform_output(): + pass + + +class ObservationPublisher(OutputInterface): + def __init__(self, topic_name, message_type): + super(ObservationPublisher, self).__init__() + + self._seq = 0; + self._publisher = rospy.Publisher( + topic_name, message_type, queue_size=10) + + def perform_output(self, msg): + try: + msg.header.stamp = rospy.get_rostime() + except Exception as exc: + pass + + self._publisher.publish(msg) + self._seq += 1 + diff --git a/src/rosgraph_monitor/output_interfaces/__init__.py b/src/rosgraph_monitor/output_interfaces/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/rosgraph_monitor/output_interfaces/diagnostic_publisher.py b/src/rosgraph_monitor/output_interfaces/diagnostic_publisher.py new file mode 100644 index 0000000..6330997 --- /dev/null +++ b/src/rosgraph_monitor/output_interfaces/diagnostic_publisher.py @@ -0,0 +1,6 @@ +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus +from rosgraph_monitor.output_interface import ObservationPublisher + +class DiagnosticPublisher(ObservationPublisher): + def __init__(self, topic_name): + super(DiagnosticPublisher, self).__init__(topic_name=topic_name, message_type=DiagnosticStatus) From ca4fcd870a4eb2aa30fdb4e754d32e114be5c5e3 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Tue, 1 Dec 2020 13:06:18 +0100 Subject: [PATCH 5/8] remove diagnostic_msgs dep --- src/rosgraph_monitor/observer.py | 7 ++----- .../output_interfaces/diagnostic_publisher.py | 6 ------ 2 files changed, 2 insertions(+), 11 deletions(-) delete mode 100644 src/rosgraph_monitor/output_interfaces/diagnostic_publisher.py diff --git a/src/rosgraph_monitor/observer.py b/src/rosgraph_monitor/observer.py index a383d78..b7b6448 100644 --- a/src/rosgraph_monitor/observer.py +++ b/src/rosgraph_monitor/observer.py @@ -1,6 +1,5 @@ import threading import rospy -from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus class Observer(object): @@ -25,13 +24,11 @@ def generate_output(): return None def _run(self): - print("starting thread") while not rospy.is_shutdown() and not self._stopped(): msg = self.generate_output() print(msg) # perform_output() should be implemented by OutputInterface's sub-class - # what would be a better way of doing this? self._perform_output(msg) self._rate.sleep() @@ -102,14 +99,14 @@ def is_close(self, a, b, rel_tol=1e-09, abs_tol=0.0): # Every derived class needs to override this def calculate_attr(self, msgs): # do calculations - return DiagnosticStatus() + return None def generate_output(self): msgs = [] received_all = True for topic, topic_type in self._topics: try: - # Add message synchronization here + # Add message synchronization here -- message filter # Consider "asynchronous" reception as well? msgs.append(rospy.wait_for_message(topic, topic_type)) except rospy.ROSException as exc: diff --git a/src/rosgraph_monitor/output_interfaces/diagnostic_publisher.py b/src/rosgraph_monitor/output_interfaces/diagnostic_publisher.py deleted file mode 100644 index 6330997..0000000 --- a/src/rosgraph_monitor/output_interfaces/diagnostic_publisher.py +++ /dev/null @@ -1,6 +0,0 @@ -from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus -from rosgraph_monitor.output_interface import ObservationPublisher - -class DiagnosticPublisher(ObservationPublisher): - def __init__(self, topic_name): - super(DiagnosticPublisher, self).__init__(topic_name=topic_name, message_type=DiagnosticStatus) From 063561f177dee4e2d835067b791648f7ed3540a1 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Tue, 1 Dec 2020 13:07:16 +0100 Subject: [PATCH 6/8] added 2 examples for observer's OutputInterface --- .../observers/dummy_diag_observer.py | 37 +++++++++++++++---- 1 file changed, 30 insertions(+), 7 deletions(-) diff --git a/src/rosgraph_monitor/observers/dummy_diag_observer.py b/src/rosgraph_monitor/observers/dummy_diag_observer.py index 82b8b36..79c5205 100644 --- a/src/rosgraph_monitor/observers/dummy_diag_observer.py +++ b/src/rosgraph_monitor/observers/dummy_diag_observer.py @@ -1,29 +1,52 @@ from rosgraph_monitor.observer import TopicObserver -from rosgraph_monitor.output_interfaces.diagnostic_publisher import DiagnosticPublisher -from std_msgs.msg import Int32 +from rosgraph_monitor.output_interface import ObservationOutputPublisher +from std_msgs.msg import Int32, Bool from diagnostic_msgs.msg import DiagnosticStatus, KeyValue -class DummyDiagnosticObserver(TopicObserver, DiagnosticPublisher): +# example observer to publish result as a DiagnosticStatus message +class DummyDiagnosticObserver(TopicObserver, ObservationOutputPublisher): def __init__(self, name): #, set_point=None): # reads from param? topics = [("/speed", Int32), ("/accel", Int32)] # list of pairs self._set_point = 5.5 #set_point - DiagnosticPublisher.__init__(self, topic_name="/diagnostics") + ObservationPublisher.__init__(self, topic_name="/diagnostics", DiagnosticStatus) TopicObserver.__init__(self, name=name, loop_rate_hz=10, topics=topics) def calculate_attr(self, msgs): status_msg = DiagnosticStatus() attr = msgs[0].data + msgs[1].data - is_close = False + is_close = True if(self._set_point): - is_close = self.is_close(attr, self._set_point) + is_close = self.is_close(attr, self._set_point, 0.05) print("{0} + {1}; is_close={2}".format(msgs[0].data, msgs[1].data, is_close)) - status_msg.level = DiagnosticStatus.OK + status_msg.level = DiagnosticStatus.OK if is_close else DiagnosticStatus.ERROR status_msg.name = self._id status_msg.values.append( KeyValue("enery", str(attr))) status_msg.message = "QA status" return status_msg + + +# example observer to publish result as a Bool message +class DummyBoolObserver(TopicObserver, ObservationPublisher): + def __init__(self, name): #, set_point=None): # reads from param? + topics = [("/speed", Int32), ("/accel", Int32)] # list of pairs + self._set_point = 5.5 #set_point + ObservationPublisher.__init__(self, topic_name="/output_topic", Bool) + TopicObserver.__init__(self, name=name, loop_rate_hz=10, topics=topics) + + def calculate_attr(self, msgs): + status_msg = DiagnosticStatus() + + attr = msgs[0].data + msgs[1].data + is_close = False + if(self._set_point): + is_close = self.is_close(attr, self._set_point, 0.05) + print("{0} + {1}; is_close={2}".format(msgs[0].data, msgs[1].data, is_close)) + + msg = Bool() + msg.data = is_close + return msg From 41e4836b6aab7ea294cb4852c29eea32c365b5d4 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Tue, 1 Dec 2020 13:07:49 +0100 Subject: [PATCH 7/8] Added plotter and logger OutputInterface --- src/rosgraph_monitor/output_interface.py | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/rosgraph_monitor/output_interface.py b/src/rosgraph_monitor/output_interface.py index 6aa5620..ced2340 100644 --- a/src/rosgraph_monitor/output_interface.py +++ b/src/rosgraph_monitor/output_interface.py @@ -9,7 +9,7 @@ def perform_output(): pass -class ObservationPublisher(OutputInterface): +class ObservationOutputPublisher(OutputInterface): def __init__(self, topic_name, message_type): super(ObservationPublisher, self).__init__() @@ -26,3 +26,20 @@ def perform_output(self, msg): self._publisher.publish(msg) self._seq += 1 + +class ObservationOutputLogger(OutputInterface): + def __init__(self, file_name): + pass + + def perform_output(self, msg): + # do logging + pass + + +class ObservationOutputPlotter(OutputInterface): + def __init__(self, plt_params): + pass + + def perform_output(self, msg): + # do plotting + pass From 2ff6a06e8e58c08dfb98003373771b6707789649 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Tue, 1 Dec 2020 13:18:46 +0100 Subject: [PATCH 8/8] added OutputInterface for ServiceObserver --- src/rosgraph_monitor/observer.py | 12 ++++-------- .../observers/graph_observer.py | 18 +++++++++++------- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/rosgraph_monitor/observer.py b/src/rosgraph_monitor/observer.py index b7b6448..ef8478a 100644 --- a/src/rosgraph_monitor/observer.py +++ b/src/rosgraph_monitor/observer.py @@ -17,9 +17,6 @@ def __del__(self): print("{} stopped".format(self._name)) # Every derived class needs to override this - # def generate_diagnostics(self): - # msg = DiagnosticArray() - # return msg def generate_output(): return None @@ -77,13 +74,12 @@ def generate_output(self): 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 + msg = self.msg_from_response(resp) + return msg # Every derived class needs to override this - # def diagnostics_from_response(self, response): - # msg = DiagnosticArray() - # return msg + def msg_from_response(self, response): + None class TopicObserver(Observer): diff --git a/src/rosgraph_monitor/observers/graph_observer.py b/src/rosgraph_monitor/observers/graph_observer.py index 5b20578..bf0040c 100644 --- a/src/rosgraph_monitor/observers/graph_observer.py +++ b/src/rosgraph_monitor/observers/graph_observer.py @@ -1,10 +1,12 @@ import imp -from rosgraph_monitor.observer import ServiceObserver -from rosgraph_monitor.parser import ModelParser -from pyparsing import * -import rospkg import os.path import re +from pyparsing import * +import rospkg +from rosgraph_monitor.observer import ServiceObserver +from rosgraph_monitor.output_interface import ObservationOutputPublisher +from rosgraph_monitor.parser import ModelParser + from ros_graph_parser.srv import GetROSModel, GetROSSystemModel from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue @@ -14,7 +16,7 @@ def strip_slash(string): return '{}'.format(string[1:] if string.startswith('/') else string) -class ROSGraphObserver(ServiceObserver): +class ROSGraphObserver(ServiceObserver, ObservationOutputPublisher): def __init__(self, name): super(ROSGraphObserver, self).__init__( name, '/get_rossystem_model', GetROSSystemModel) @@ -23,7 +25,8 @@ def __init__(self, name): self.model_path = os.path.join(rospack.get_path('rosgraph_monitor'), "resources/cob4-25.rossystem") self._rossystem_parser = ModelParser(self.model_path) - def diagnostics_from_response(self, resp): + def msg_from_response(self, resp): + diag_array = DiagnosticArray() status_msgs = list() if resp is None: return status_msgs @@ -69,7 +72,8 @@ def diagnostics_from_response(self, resp): KeyValue(params[0], str(params[1]))) status_msgs.append(status_msg) - return status_msgs + diag_msg.status.extend(status_msgs) + return diag_msg # find out missing and additional interfaces # if both lists are empty, system is running fine