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 math import floor
00037 import os
00038
00039 import roslib;roslib.load_manifest('rqt_robot_monitor')
00040 import rospy
00041 from python_qt_binding import loadUi
00042 from python_qt_binding.QtCore import QPointF, QSize, Qt, Signal
00043 from python_qt_binding.QtGui import QColor, QIcon, QGraphicsScene, QWidget
00044
00045 from .timeline import TimelineView
00046 from .util_robot_monitor import Util
00047
00048 class TimelinePane(QWidget):
00049 """
00050 This class defines the pane where timeline, button, message are displayed.
00051 """
00052
00053 _sig_update = Signal()
00054
00055 def __init__(self, parent, len_timeline,
00056 msg_callback,
00057 color_callback,
00058 pause_callback=None):
00059 """
00060
00061 @param msg_callback: This can be a function that takes either
00062 { DiagnosticArray, DiagnosticsStatus } as an argument. If your timeline
00063 is to show the status of all devices, choose the former.
00064 Else (eg. your timeline only handles the status of an arbitrary
00065 node) use the latter.
00066
00067 @param color_callback: Not directly used within this class. Instead,
00068 this will be passed and used in TimelineView class.
00069
00070 @todo: pause_callback is MUST since it's used in both by
00071 RobotMonitorWidget & InspectorWindow.
00072 """
00073
00074 super(TimelinePane, self).__init__()
00075 self._parent = parent
00076
00077 ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
00078 'rqt_robot_monitor_timelinepane.ui')
00079 loadUi(ui_file, self, {'TimelineView': TimelineView})
00080
00081
00082
00083 self._pause_callback = pause_callback
00084 self._msg_callback = msg_callback
00085
00086 self._timeline_view.set_init_data(1, len_timeline, 5,
00087
00088 color_callback)
00089
00090 self._scene = QGraphicsScene(self._timeline_view)
00091
00092 self._timeline_view.setScene(self._scene)
00093 self._timeline_view.show()
00094
00095
00096
00097 self._queue_diagnostic = deque()
00098 self._len_timeline = len_timeline
00099 self._paused = False
00100 self._tracking_latest = True
00101 self._last_sec_marker_at = 2
00102 self._last_msg = None
00103
00104 self._pause_button.clicked[bool].connect(self._pause)
00105 self._sig_update.connect(self._timeline_view.slot_redraw)
00106
00107 def mouse_release(self, event):
00108 """
00109 @param event: QMouseEvent
00110 """
00111 xpos_clicked = event.x()
00112 width_each_cell_shown = float(
00113 self._timeline_view.viewport().width()) / len(
00114 self._queue_diagnostic)
00115 i = int(floor(xpos_clicked / width_each_cell_shown))
00116 rospy.logdebug('mouse_release i=%d width_each_cell_shown=%s',
00117 i, width_each_cell_shown)
00118
00119
00120 msg = self._queue_diagnostic[i]
00121 if msg:
00122 self._parent.on_pause(True, msg)
00123
00124 if not self._pause_button.isChecked():
00125 self._pause_button.toggle()
00126
00127
00128
00129
00130
00131
00132
00133 def get_worst(self, msg):
00134 lvl = 0
00135 for status in msg.status:
00136 if status.level > lvl:
00137 lvl = status.level
00138 return lvl
00139
00140 def _pause(self, paused):
00141 """
00142 Should be the only interface for
00143 pausing timeline pane and timeline itself (which is not as of now..).
00144
00145 @todo: Think about optimal way to call functions in parent class
00146 (pause, _unpause). Use callback or just calling via parent instance?
00147
00148 @param paused: bool
00149 """
00150
00151 rospy.logdebug('TimelinePane pause isPaused?=%s', paused)
00152
00153 if paused:
00154 self._pause_button.setDown(True)
00155 self._paused = True
00156 self._tracking_latest = False
00157
00158 self._parent.pause(self._queue_diagnostic[-1])
00159 else:
00160 self._pause_button.setDown(False)
00161 self._paused = False
00162 self._tracking_latest = True
00163 self._parent.unpause(self._queue_diagnostic[-1])
00164 rospy.logdebug('Objs; parent=%s, parent._unpause obj=%s',
00165 self._parent,
00166 self._parent.unpause)
00167
00168
00169
00170
00171 def on_slider_scroll(self, evt):
00172 """
00173
00174 Copied from robot_monitor.
00175
00176 @param evt: QMouseEvent
00177
00178 """
00179 xpos_marker = self._timeline_view.get_xpos_marker() - 1
00180 rospy.logdebug('on_slider_scroll xpos_marker=%s _last_sec_marker_at=%s',
00181 xpos_marker, self._last_sec_marker_at)
00182 if (xpos_marker == self._last_sec_marker_at):
00183
00184 return
00185
00186 elif (xpos_marker >= len(self._queue_diagnostic)):
00187
00188 return
00189
00190 self._last_sec_marker_at = xpos_marker
00191
00192
00193
00194
00195
00196 self._pause(True)
00197
00198
00199
00200
00201
00202
00203 msg = self._queue_diagnostic[xpos_marker]
00204 if self._msg_callback:
00205 self._msg_callback(msg, True)
00206
00207 def new_diagnostic(self, msg):
00208 """
00209 Callback for new msg for TimelinePane class.
00210
00211 Puts new msg into a queue, update the length of timeline. Also emits
00212 a signal to notify another callbacks.
00213 This ignores new msg if timeline is paused.
00214
00215 @param msg: Either DiagnosticArray or DiagnosticsStatus. Can be
00216 determined by __init__'s arg "msg_callback".
00217
00218 Copied from robot_monitor.
00219 """
00220
00221 self._last_msg = msg
00222
00223
00224
00225
00226
00227
00228
00229 if (self._paused):
00230 return
00231
00232 self._queue_diagnostic.append(msg)
00233 if (len(self._queue_diagnostic) > self._len_timeline):
00234 self._queue_diagnostic.popleft()
00235
00236 new_len = len(self._queue_diagnostic)
00237 self._timeline_view.set_range(1, new_len)
00238
00239
00240
00241
00242
00243
00244 self._sig_update.emit()
00245 rospy.logdebug(' TimelinePane new_diagnostic new_len=%d', new_len)
00246
00247 def redraw(self):
00248 self._sig_update.emit()
00249
00250 def get_diagnostic_queue(self):
00251 """
00252
00253 @return: a queue that contains either DiagnosticArray or
00254 DiagnosticsStatus. Depends on the purpose.
00255
00256 Copied from robot_monitor.
00257 """
00258
00259 return self._queue_diagnostic