Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 from collections import deque
00036 from python_qt_binding.QtCore import Signal, Slot, QObject
00037
00038 import rospy
00039
00040 from diagnostic_msgs.msg import DiagnosticArray
00041
00042
00043 class Timeline(QObject):
00044 """
00045 A class which represents the status history of diagnostics
00046 It can be queried for a past history of diagnostics, and paused
00047 """
00048 message_updated = Signal(dict)
00049 queue_updated = Signal()
00050 pause_changed = Signal(bool)
00051 position_changed = Signal(int)
00052
00053 def __init__(self, topic, topic_type, count=30):
00054 super(Timeline, self).__init__()
00055 self._queue = deque(maxlen=count)
00056 self._count = count
00057 self._current_index = -1
00058
00059
00060
00061
00062 self._paused_queue = None
00063
00064 self._last_message_time = 0
00065
00066 self._subscriber = rospy.Subscriber(topic, topic_type, self.callback,
00067 queue_size=10)
00068
00069 def shutdown(self):
00070 """
00071 Turn off this Timeline
00072 Internally, this just shuts down the subscriber
00073 """
00074 self._subscriber.unregister()
00075
00076 @Slot(bool)
00077 def set_paused(self, pause):
00078 """
00079 Slot, to be called to change the pause status of the timeline
00080
00081 This is generally intended to be connected to the status signal
00082 from a button or checkbox
00083 """
00084 if pause != self.paused:
00085 if pause:
00086 self._paused_queue = deque(self._queue, self._queue.maxlen)
00087 else:
00088 self._queue = self._paused_queue
00089 self._paused_queue = None
00090
00091
00092 self._current_index = -1
00093 self.message_updated.emit(self._queue[self.position])
00094 self.pause_changed.emit(pause)
00095
00096 @property
00097 def paused(self):
00098 """ True if this timeline is paused """
00099 return self._paused_queue is not None
00100
00101 def callback(self, msg):
00102 """
00103 ROS Callback for new diagnostic messages
00104
00105 Puts new msg into the queue, and emits a signal to let listeners know
00106 that the timeline has been updated
00107
00108 If the timeline is paused, new messages are placed into a separate
00109 queue and swapped back in when the timeline is unpaused
00110
00111 :type msg: Either DiagnosticArray or DiagnosticsStatus. Can be
00112 determined by __init__'s arg "msg_callback".
00113 """
00114 self._last_message_time = rospy.get_time()
00115 dic = {status.name: status for status in msg.status}
00116
00117 if self.paused:
00118 self._paused_queue.append(dic)
00119 else:
00120 self._queue.append(dic)
00121 self.queue_updated.emit()
00122 if self.position == -1:
00123 self.message_updated.emit(dic)
00124
00125 @property
00126 def has_messages(self):
00127 """
00128 True if this timeline has received any messages.
00129 False if no messages have been received yet
00130 """
00131 return len(self._queue) > 0
00132
00133 @property
00134 def data_age(self):
00135 """ Get the age (in seconds) of the most recent diagnostic message """
00136 current_time = rospy.get_time()
00137 time_diff = current_time - self._last_message_time
00138 return time_diff
00139
00140 @property
00141 def is_stale(self):
00142 """ True is this timeline is stale. """
00143 return self.data_age > 10.0
00144
00145 @property
00146 def position(self):
00147 index = self._current_index
00148 while index < -1:
00149 index = len(self._queue) + index
00150 if index == len(self._queue) - 1:
00151 index = -1
00152 return index
00153
00154 @position.setter
00155 def position(self, index):
00156 max_index = len(self._queue) - 1
00157 min_index = -len(self._queue)
00158 index = min(index, max_index)
00159 index = max(index, min_index)
00160 if index != self._current_index or self._current_index == -1:
00161 self._current_index = index
00162 self.message_updated.emit(self._queue[index])
00163
00164 @Slot(int)
00165 def set_position(self, position):
00166 self.position = position
00167 self.position_changed.emit(position)
00168
00169 def get_current_status_by_name(self, name):
00170 return self._queue[self.position][name]
00171
00172 def get_all_status_by_name(self, name):
00173 return [status[name] for status in list(self._queue)]
00174
00175 def __len__(self):
00176 return len(self._queue)
00177
00178 def __iter__(self):
00179 for msg in list(self._queue):
00180 yield msg
rqt_robot_monitor
Author(s): Austin Hendrix, Isaac Saito, Ze'ev Klapow, Kevin Watts, Josh Faust
autogenerated on Thu May 16 2019 03:10:09