$search
00001 #!/usr/bin/env python 00002 # Software License Agreement (BSD License) 00003 # 00004 # Copyright (c) 2009, Willow Garage, Inc. 00005 # All rights reserved. 00006 # 00007 # Redistribution and use in source and binary forms, with or without 00008 # modification, are permitted provided that the following conditions 00009 # are met: 00010 # 00011 # * Redistributions of source code must retain the above copyright 00012 # notice, this list of conditions and the following disclaimer. 00013 # * Redistributions in binary form must reproduce the above 00014 # copyright notice, this list of conditions and the following 00015 # disclaimer in the documentation and/or other materials provided 00016 # with the distribution. 00017 # * Neither the name of the Willow Garage nor the names of its 00018 # contributors may be used to endorse or promote products derived 00019 # from this software without specific prior written permission. 00020 # 00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 # POSSIBILITY OF SUCH DAMAGE. 00033 # 00034 00035 ##\author Kevin Watts, Josh Faust 00036 00037 PKG = 'robot_monitor' 00038 00039 import roslib; roslib.load_manifest(PKG) 00040 00041 import sys, os 00042 import rospy 00043 00044 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue 00045 00046 import wx 00047 from wx import xrc 00048 from wx import html 00049 from wx import richtext 00050 00051 import copy 00052 00053 import cStringIO 00054 00055 from message_timeline import MessageTimeline 00056 00057 stat_dict = {0: 'OK', 1: 'Warning', 2: 'Error', 3: 'Stale' } 00058 color_dict = {0: wx.Colour(85, 178, 76), 1: wx.Colour(222, 213, 17), 2: wx.Colour(178, 23, 46), 3: wx.Colour(178, 23, 46)} 00059 00060 class SnapshotFrame(wx.Frame): 00061 def __init__(self, parent, name): 00062 wx.Frame.__init__(self, parent, wx.ID_ANY, "Snapshot of %s"%(name)) 00063 00064 self._sizer = wx.BoxSizer(wx.VERTICAL) 00065 self._text_ctrl = richtext.RichTextCtrl(self, wx.ID_ANY, wx.EmptyString, wx.DefaultPosition, wx.DefaultSize, wx.TE_READONLY) 00066 self._sizer.Add(self._text_ctrl, 1, wx.EXPAND) 00067 00068 class Item(): 00069 def __init__(self): 00070 self.name = None 00071 00072 def __cmp__(self, other): 00073 if (isinstance(other, str)): 00074 return cmp(self.name, other) 00075 else: 00076 return cmp(self.name, other.name) 00077 00078 ##\brief Frame views status messages in separate window 00079 ## 00080 ##\todo Don't initialize it on top of main frame somehow 00081 class StatusViewerFrame(wx.Frame): 00082 ##\param parent RobotMonitorFrame : Parent frame 00083 ##\param name str : Full topic name 00084 ##\param manager RobotMonitor : Manager of frame 00085 ##\param title str: Frame title 00086 def __init__(self, parent, name, manager, title): 00087 wx.Frame.__init__(self, parent, wx.ID_ANY, title) 00088 00089 self._sizer = wx.BoxSizer(wx.VERTICAL) 00090 00091 self._text_ctrl = richtext.RichTextCtrl(self, wx.ID_ANY, wx.EmptyString, wx.DefaultPosition, wx.DefaultSize, wx.TE_READONLY) 00092 self._sizer.Add(self._text_ctrl, 1, wx.EXPAND) 00093 00094 bottom_sizer = wx.BoxSizer(wx.HORIZONTAL) 00095 self._timeline = MessageTimeline(self, 30, None, None, self._write_status, self._get_color_for_message, None) 00096 bottom_sizer.Add(self._timeline, 1, wx.EXPAND|wx.ALL, 5) 00097 00098 self._snapshot_button = wx.Button(self, wx.ID_ANY, "Snapshot") 00099 self._snapshot_button.Bind(wx.EVT_BUTTON, self._on_snapshot) 00100 self._snapshot_button.SetToolTip(wx.ToolTip("Freeze data in new window")) 00101 bottom_sizer.Add(self._snapshot_button, 0, wx.ALL|wx.ALIGN_CENTER_VERTICAL, 5) 00102 00103 self._sizer.Add(bottom_sizer, 0, wx.EXPAND) 00104 00105 self.SetSizer(self._sizer) 00106 00107 self._text_ctrl.SetFocus() 00108 00109 self._manager = manager 00110 self._name = name 00111 00112 self._default_style = self._text_ctrl.GetDefaultStyle() 00113 self._basic_style = self._text_ctrl.GetBasicStyle() 00114 00115 self.Bind(wx.EVT_CHAR, self.on_char) 00116 self._text_ctrl.Bind(wx.EVT_CHAR, self.on_char) 00117 00118 self.Bind(wx.EVT_CLOSE, self._on_close) 00119 00120 self._last_values = {} 00121 self._last_status = None 00122 00123 self._items = [] 00124 00125 def _on_close(self, event): 00126 event.Skip() 00127 self._manager.remove_viewer(self._name) 00128 00129 def _on_snapshot(self, event): 00130 snapshot = SnapshotFrame(self, self._name) 00131 snapshot._text_ctrl.SetValue(self._text_ctrl.GetValue()) 00132 snapshot.Show(True) 00133 snapshot.Raise() 00134 snapshot.Center() 00135 00136 def set_status(self, status): 00137 if (self._timeline.IsEnabled()): 00138 self._timeline.add_msg(status) 00139 else: 00140 self._write_status(status) 00141 00142 def _set_kv(self, key, value, changed=False): 00143 self._text_ctrl.BeginBold() 00144 self._text_ctrl.WriteText("%s: "%(key)) 00145 self._text_ctrl.EndBold() 00146 00147 if (not changed): 00148 self._text_ctrl.WriteText(value) 00149 else: 00150 self._text_ctrl.BeginBold() 00151 self._text_ctrl.BeginTextColour(wx.Colour(0xaa, 0x77, 0x00)) 00152 self._text_ctrl.WriteText(value) 00153 self._text_ctrl.EndTextColour() 00154 self._text_ctrl.EndBold() 00155 self._text_ctrl.Newline() 00156 00157 def _write_status(self, status): 00158 self._text_ctrl.Freeze() 00159 self._text_ctrl.BeginSuppressUndo() 00160 00161 # SetCaretPosition and GetCaretPosition was only added to wxPython in 2.8.10 apparently, even though they've been in wx for ages 00162 has_caret_accessors = True 00163 try: 00164 getattr(self._text_ctrl, 'SetCaretPosition') 00165 except: 00166 has_caret_accessors = False 00167 00168 #has_caret_accessors = False 00169 00170 if (has_caret_accessors): 00171 self._text_ctrl.SetCaretPosition(0) 00172 else: 00173 self._text_ctrl.Clear() 00174 00175 self._text_ctrl.SetBasicStyle(self._basic_style) 00176 self._text_ctrl.SetDefaultStyle(self._default_style) 00177 self._set_kv("Full name", status.name) 00178 self._set_kv("Component", status.name.split('/')[-1]) 00179 self._set_kv("Hardware ID", status.hardware_id) 00180 self._set_kv("Level", stat_dict[status.level], self._last_status is not None and self._last_status.level != status.level) 00181 self._set_kv("Message", status.message, self._last_status is not None and self._last_status.message != status.message) 00182 self._text_ctrl.Newline() 00183 00184 for value in status.values: 00185 changed = False 00186 if (self._last_values.has_key(value.key) and self._last_values[value.key] != value.value): 00187 changed = True 00188 00189 self._set_kv(value.key, value.value, changed) 00190 self._last_values[value.key] = value.value 00191 00192 self._text_ctrl.EndAllStyles() 00193 00194 if (has_caret_accessors): 00195 self._text_ctrl.Remove(self._text_ctrl.GetCaretPosition(), self._text_ctrl.GetLastPosition()) 00196 00197 self._text_ctrl.EndSuppressUndo() 00198 self._text_ctrl.Thaw() 00199 00200 self._last_status = status 00201 00202 def _get_color_for_message(self, msg): 00203 return color_dict[msg.level] 00204 00205 def on_char(self, evt): 00206 if (evt.GetKeyCode() == wx.WXK_ESCAPE): 00207 self.Close() 00208 else: 00209 evt.Skip() 00210 00211 def disable_timeline(self): 00212 self._timeline.disable() 00213 self._timeline.clear() 00214 00215 def enable_timeline(self): 00216 self._timeline.enable() 00217 00218 def get_name(self): 00219 return self._name