time_pane.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2012, Willow Garage, Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions
00008 # are met:
00009 #
00010 #  * Redistributions of source code must retain the above copyright
00011 #    notice, this list of conditions and the following disclaimer.
00012 #  * Redistributions in binary form must reproduce the above
00013 #    copyright notice, this list of conditions and the following
00014 #    disclaimer in the documentation and/or other materials provided
00015 #    with the distribution.
00016 #  * Neither the name of Willow Garage, Inc. nor the names of its
00017 #    contributors may be used to endorse or promote products derived
00018 #    from this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 # POSSIBILITY OF SUCH DAMAGE.
00032 #
00033 # Author: Isaac Saito, Ze'ev Klapow
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         # Initialization process for TimeLineView obj continues.
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                                            # self._get_color_for_value)
00088                                            color_callback)
00089   
00090         self._scene = QGraphicsScene(self._timeline_view)
00091         # self._colors = [QColor('green'), QColor('yellow'), QColor('red')]
00092         self._timeline_view.setScene(self._scene)        
00093         self._timeline_view.show()
00094         
00095         # self._messages = [None for x in range(len_timeline)]  # DiagnosticStatus
00096         # self._queue_diagnostic = [1 for x in range(20)] 
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         # msg = self._messages[i]
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     # def resizeEvent(self, event):
00128     #    """Override."""
00129         # self._slot_redraw()
00130     #    self._timeline_view.update()        
00131     #    rospy.loginfo('TimelinePane resizeEvent!!!!!!')
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             # self._parent._pause(self._messages[-1])
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             #if (self._last_msg is not None):
00169             #self.new_diagnostic(self._last_msg)
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             # Clicked the same pos as last time.
00184             return
00185         # if (xpos_marker >= len(self._queue)):
00186         elif (xpos_marker >= len(self._queue_diagnostic)):
00187             # When clicked out-of-region
00188             return
00189 
00190         self._last_sec_marker_at = xpos_marker
00191         
00192         # # TODO Pause InspectionWindow if any.
00193         # if (not self._paused and self._pause_callback is not None):            
00194             # self._pause_callback(True)
00195             
00196         self._pause(True)
00197         # self._pause_button.SetBackgroundColour(wx.Colour(123, 193, 255))
00198         
00199         #self._paused = True
00200         
00201         # Fetch corresponding previous DiagsnoticArray instance from queue,
00202         # and _sig_update trees.
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  # Shouldn't this sit after self._paused return?
00222                              # Original robot_monitor does this way.   
00223         
00224         # if (self._message_receipt_callback is not None):
00225         #    self._message_receipt_callback(msg)
00226         # # This is done here in robot_monitor. 
00227         # # In rqt_robot_monitor, however, it's done in RobotMonitorWidget.
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 #        if (self._tracking_latest):                 
00240 #            self._timeline_view._set_xpos_marker(new_len)         
00241 #        else:
00242 #            rospy.loginfo('In MessageTimeline _new_msg not calling redraw')
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         # return self._queue
00259         return self._queue_diagnostic


rqt_robot_monitor
Author(s): Isaac Saito, Ze'ev Klapow
autogenerated on Fri Jan 3 2014 11:56:17