Go to the documentation of this file.00001
00002 import rospy
00003 import smach
00004
00005 from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached
00006 from flexbe_msgs.msg import CommandFeedback, OutcomeRequest
00007 from flexbe_core.logger import Logger
00008
00009 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
00010
00011
00012 class MonitoringState(smach.State):
00013 """
00014 A state to monitor a custom set of conditions.
00015 For each conditions, an outcome is added or mapped which will be returned if the condition is not met.
00016 """
00017
00018 def __init__(self, *args, **kwargs):
00019 super(MonitoringState, self).__init__(*args, **kwargs)
00020
00021 outcomes = kwargs.get('outcomes', [])
00022 self._outcome_list = list(outcomes)
00023 self._outcome_list.remove('loopback')
00024
00025 self.name = None
00026 self._parent = None
00027 self._is_controlled = False
00028 self._force_monitoring = False
00029
00030 self._monitored_keys = dict()
00031 self._sent_keys = list()
00032 self._current_status = None
00033
00034 self.__execute = self.execute
00035 self.execute = self._monitoring_execute
00036
00037 self._diagnostics_topic = 'diagnostics_agg'
00038 self._sub = ProxySubscriberCached()
00039
00040
00041 def _monitoring_execute(self, *args, **kwargs):
00042 new_status = None
00043 had_warning = False
00044 if (self._force_monitoring or self._is_controlled) and self._sub.has_buffered(self._diagnostics_topic):
00045 new_status = ""
00046 diag_msg = self._sub.get_from_buffer(self._diagnostics_topic)
00047 for status in diag_msg.status:
00048 if not status.name in self._monitored_keys.keys(): continue
00049 if status.level == DiagnosticStatus.WARN:
00050 had_warning = True
00051 if not status.name + "_warn" in self._sent_keys:
00052 self._sent_keys.append(status.name + "_warn")
00053 Logger.logwarn("%s: %s" % (status.name, status.message))
00054 if status.level == DiagnosticStatus.ERROR:
00055 if not status.name + "_err" in self._sent_keys:
00056 self._sent_keys.append(status.name + "_err")
00057 Logger.logerr("%s: %s" % (status.name, status.message))
00058 new_status = status.name
00059
00060 if new_status == "":
00061 self._current_status = None
00062 new_status = None
00063 if not had_warning:
00064 self._sent_keys = list()
00065
00066 if new_status is None or self._current_status == new_status:
00067 return self.__execute(*args, **kwargs)
00068
00069 self._current_status = new_status
00070
00071 return self._monitored_keys[self._current_status]
00072
00073
00074 def monitor(self, key, outcome = None):
00075 oc = outcome if not outcome is None else key
00076 self._monitored_keys[key] = oc
00077 if not oc in self._outcomes:
00078 self.register_outcomes([oc])
00079 self._outcome_list.append(oc)
00080
00081 def force_monitoring(self):
00082 self._force_monitoring = True
00083 if not self._is_controlled:
00084 self._sub.subscribe(self._diagnostics_topic, DiagnosticArray)
00085 self._sub.enable_buffer(self._diagnostics_topic)
00086
00087 def _enable_ros_control(self):
00088 self._is_controlled = True
00089 self._sub.subscribe(self._diagnostics_topic, DiagnosticArray)
00090 self._sub.enable_buffer(self._diagnostics_topic)
00091
00092 def _disable_ros_control(self):
00093 self._is_controlled = False
00094 self._sub.unsubscribe_topic(self._diagnostics_topic)
00095
00096
00097 def _get_path(self):
00098 return self._parent._get_path() + "/" + self.name