robot_monitor_panel.py
Go to the documentation of this file.
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 
00049 import threading, time
00050 
00051 from viewer_panel import StatusViewerFrame
00052 from robot_monitor_generated import MonitorPanelGenerated
00053 from message_timeline import MessageTimeline
00054 
00055 color_dict = {0: wx.Colour(85, 178, 76), 1: wx.Colour(222, 213, 17), 2: wx.Colour(178, 23, 46), 3: wx.Colour(40, 23, 176)}
00056 error_levels = [2, 3]
00057 
00058 def get_nice_name(status_name):
00059     return status_name.split('/')[-1]
00060 
00061 def get_parent_name(status_name):
00062     return ('/'.join(status_name.split('/')[:-1])).strip()
00063 
00064 class StatusItem(object):
00065     def __init__(self, status):
00066         self.tree_id = None
00067         self.warning_id = None
00068         self.error_id = None
00069         self.last_level = None
00070         self.update(status)
00071         
00072     def update(self, status):
00073         self.status = status
00074         
00075 class State(object):
00076     def __init__(self):
00077         self._items = {}
00078         self._msg = None
00079         self._has_warned_no_name = False
00080 
00081     def reset(self):
00082         self._items = {}
00083         self._msg = None
00084         
00085     def get_parent(self, item):
00086         parent_name = get_parent_name(item.status.name)
00087         
00088         if (parent_name not in self._items):
00089             return None
00090         
00091         return self._items[parent_name]
00092     
00093     def get_descendants(self, item):
00094         child_keys = [k for k in self._items.iterkeys() if k.startswith(item.status.name + "/")]
00095         children = [self._items[k] for k in child_keys]
00096         return children
00097     
00098     def get_items(self):
00099         return self._items
00100         
00101     def update(self, msg):
00102         removed = []
00103         added = []
00104         items = {}
00105         
00106         # fill items from new msg, creating new StatusItems for any that don't already exist,
00107         # and keeping track of those that have been added new
00108         for s in msg.status:
00109             # DiagnosticStatus messages without a name are invalid #3806
00110             if not s.name and not self._has_warned_no_name:
00111                 rospy.logwarn('DiagnosticStatus message with no "name". Unable to add to robot monitor. Message: %s, hardware ID: %s, level: %d' % (s.message, s.hardware_id, s.level))
00112                 self._has_warned_no_name = True
00113 
00114             if not s.name:
00115                 continue
00116                 
00117             if (len(s.name) > 0 and s.name[0] != '/'):
00118                 s.name = '/' + s.name
00119 
00120             if (s.name not in self._items):
00121                 i = StatusItem(s)
00122                 added.append(i)
00123                 items[s.name] = i
00124             else:
00125                 i = self._items[s.name]
00126                 i.update(s)
00127                 items[s.name] = i
00128         
00129         # find anything without a parent already in the items, and add it as a dummy
00130         # item
00131         to_add = []
00132         dummy_names = []
00133         for i in items.itervalues():
00134             parent = i.status.name
00135             while (len(parent) != 0):
00136                 parent = get_parent_name(parent)
00137                 if (len(parent) > 0 and parent not in items and parent not in dummy_names):
00138                     pi = None
00139                     if (parent not in self._items):
00140                         s = DiagnosticStatus()
00141                         s.name = parent
00142                         s.message = ""
00143                         pi = StatusItem(s)
00144                     else:
00145                         pi = self._items[parent]
00146                         
00147                     to_add.append(pi)
00148                     dummy_names.append(pi.status.name)
00149                   
00150         for a in to_add:
00151             if (a.status.name not in items):
00152                 items[a.status.name] = a
00153                 
00154                 if (a.status.name not in self._items):
00155                     added.append(a)
00156         
00157         for i in self._items.itervalues():
00158             # determine removed items
00159             if (i.status.name not in items):
00160                 removed.append(i)
00161         
00162         # remove removed items
00163         for r in removed:
00164             del self._items[r.status.name]
00165         
00166         self._items = items
00167         self._msg = msg
00168         
00169         # sort so that parents are always added before children
00170         added.sort(cmp=lambda l,r: cmp(l.status.name, r.status.name))
00171         # sort so that children are always removed before parents
00172         removed.sort(cmp=lambda l,r: cmp(l.status.name, r.status.name), reverse=True)
00173         
00174         #added_keys = [a.status.name for a in added]
00175         #if len(added_keys) > 0: print "Added: ", added_keys
00176         #removed_keys = [r.status.name for r in removed]
00177         #if (len(removed_keys) > 0): print "Removed: ", removed_keys
00178         
00179         return (added, removed, self._items)
00180 
00181 ##\brief Monitor panel for aggregated diagnostics (diagnostics_agg)
00182 ##
00183 ## Displays data from DiagnosticArray diagnostics_agg in a tree structure
00184 ## by status name. Names are parsed by '/'. Each status name is given
00185 ## an icon by status (ok, warn, error, stale).
00186 ## 
00187 ## The robot monitor does not store any state, but if it does not get any updates 
00188 ## for 3 seconds, it will mark the tree as stale.
00189 class RobotMonitorPanel(MonitorPanelGenerated):
00190     ##\param parent RobotMonitorFrame : Parent frame
00191     def __init__(self, parent, rxbag = False):
00192         MonitorPanelGenerated.__init__(self, parent)
00193 
00194         self._frame = parent
00195         self._rxbag = rxbag
00196 
00197         self._tree_ctrl.AddRoot("Root")
00198         self._error_tree_ctrl.AddRoot("Root")
00199         self._warning_tree_ctrl.AddRoot("Root")
00200         
00201         self._tree_ctrl.SetToolTip(wx.ToolTip("Double click an item for details"))
00202         self._error_tree_ctrl.SetToolTip(wx.ToolTip("Double click an item for details"))
00203         self._warning_tree_ctrl.SetToolTip(wx.ToolTip("Double click an item for details"))
00204 
00205         # Image list for icons
00206         image_list = wx.ImageList(16, 16)
00207         error_id = image_list.AddIcon(wx.ArtProvider.GetIcon(wx.ART_ERROR, wx.ART_OTHER, wx.Size(16, 16)))
00208         warn_id = image_list.AddIcon(wx.ArtProvider.GetIcon(wx.ART_WARNING, wx.ART_OTHER, wx.Size(16, 16)))
00209         ok_id = image_list.AddIcon(wx.ArtProvider.GetIcon(wx.ART_TICK_MARK, wx.ART_OTHER, wx.Size(16, 16)))
00210         stale_icon = wx.Icon(os.path.join(roslib.packages.get_pkg_dir(PKG), 'icons/stale.png'), wx.BITMAP_TYPE_PNG)
00211         stale_id = image_list.AddIcon(stale_icon)
00212         self._tree_ctrl.SetImageList(image_list)
00213         self._error_tree_ctrl.SetImageList(image_list)
00214         self._warning_tree_ctrl.SetImageList(image_list)
00215         self._image_list = image_list
00216         
00217         # Tell users we don't have any items yet
00218         self._image_dict = { 0: ok_id, 1: warn_id, 2: error_id, 3: stale_id }
00219 
00220         # Bind double click event
00221         self._tree_ctrl.Bind(wx.EVT_TREE_ITEM_ACTIVATED, self.on_all_item_activate)
00222         self._error_tree_ctrl.Bind(wx.EVT_TREE_ITEM_ACTIVATED, self.on_error_item_activate)
00223         self._warning_tree_ctrl.Bind(wx.EVT_TREE_ITEM_ACTIVATED, self.on_warning_item_activate)
00224         self._viewers = {}
00225 
00226         self._state = State()
00227 
00228         # Reset monitor to starting configuration
00229         self._have_message = False
00230         self._empty_id = None
00231         self.reset_monitor()
00232 
00233         # Show stale with timer
00234         self._timer = wx.Timer(self)
00235         self.Bind(wx.EVT_TIMER, self._update_message_state)
00236         self._timer.Start(1000)
00237 
00238         self._timeline = MessageTimeline(self, 30, "diagnostics_agg", DiagnosticArray, self.new_message, self.get_color_for_message, self._on_pause)
00239         self._timeline.set_message_receipt_callback(self._on_new_message_received)
00240         self.GetSizer().Add(self._timeline, 0, wx.EXPAND)
00241         
00242         # unfortunately tooltips do not work on static text, so this information has to go into the docs only
00243         #self._message_status_text.SetToolTip(wx.ToolTip("""Shows the status of the received aggregated diagnostic message.
00244 
00245     def Close(self):
00246         self._timeline.Close()
00247         MonitorPanelGenerated.Close(self)
00248 
00249 #If the robot monitor has not received a diagnostic message in more than 10 seconds, this will show an error and the background of the Errors/Warnings/All views will turn grey."""))
00250 
00251     ##\brief Sets robot monitor messages status in default start configuration
00252     def _set_initial_message_state(self):
00253         if self._rxbag:
00254             self._message_status_text.SetLabel("No message received")
00255             self._message_status_text.SetForegroundColour(color_dict[1])
00256             self._tree_ctrl.SetBackgroundColour(wx.LIGHT_GREY)
00257             self._error_tree_ctrl.SetBackgroundColour(wx.LIGHT_GREY)
00258             self._warning_tree_ctrl.SetBackgroundColour(wx.LIGHT_GREY)
00259         else:
00260             self._message_status_text.SetLabel("No message received")
00261             self._message_status_text.SetForegroundColour(color_dict[2])
00262             self._tree_ctrl.SetBackgroundColour(wx.LIGHT_GREY)
00263             self._error_tree_ctrl.SetBackgroundColour(wx.LIGHT_GREY)
00264             self._warning_tree_ctrl.SetBackgroundColour(wx.LIGHT_GREY)
00265 
00266     ##\brief Updates status bar with status of diagnostics_agg topic
00267     def _update_message_state(self, event = None):
00268         if not self._have_message:
00269             self._set_initial_message_state()
00270             return
00271         if self._rxbag:
00272             self._message_status_text.SetLabel("Receiving rxbag messages")
00273             self._message_status_text.SetForegroundColour(color_dict[0])
00274             self._tree_ctrl.SetBackgroundColour(wx.WHITE)
00275             self._error_tree_ctrl.SetBackgroundColour(wx.WHITE)
00276             self._warning_tree_ctrl.SetBackgroundColour(wx.WHITE)
00277             self._is_stale = False
00278             return # Return so we don't call get_time
00279 
00280         current_time = rospy.get_time()
00281         time_diff = current_time - self._last_message_time
00282         if (time_diff > 10.0):
00283             self._message_status_text.SetLabel("Last message received %s seconds ago"%(int(time_diff)))
00284             self._message_status_text.SetForegroundColour(color_dict[2])
00285             self._tree_ctrl.SetBackgroundColour(wx.LIGHT_GREY)
00286             self._error_tree_ctrl.SetBackgroundColour(wx.LIGHT_GREY)
00287             self._warning_tree_ctrl.SetBackgroundColour(wx.LIGHT_GREY)
00288             self._is_stale = True
00289         else:
00290             seconds_string = "seconds"
00291             if (int(time_diff) == 1):
00292                 seconds_string = "second"
00293             self._message_status_text.SetLabel("Last message received %s %s ago"%(int(time_diff), seconds_string))
00294             self._message_status_text.SetForegroundColour(color_dict[0])
00295             self._tree_ctrl.SetBackgroundColour(wx.WHITE)
00296             self._error_tree_ctrl.SetBackgroundColour(wx.WHITE)
00297             self._warning_tree_ctrl.SetBackgroundColour(wx.WHITE)
00298             self._is_stale = False
00299         
00300     
00301     ## \brief Called whenever a new message is received by the timeline.  Different from new_message in that it
00302     ## is called even if the timeline is paused, and only when a new message is received, not when the timeline
00303     ## is scrubbed
00304     def _on_new_message_received(self, msg):
00305         self._last_message_time = rospy.get_time()
00306 
00307     ##\brief Clears messages at startup, or for rxbag plugin
00308     def reset_monitor(self):
00309         # Reset tree control
00310         if self._have_message:
00311             self._tree_ctrl.DeleteChildren(self._tree_ctrl.GetRootItem())
00312             self._error_tree_ctrl.DeleteChildren(self._error_tree_ctrl.GetRootItem())
00313             self._warning_tree_ctrl.DeleteChildren(self._warning_tree_ctrl.GetRootItem())
00314             self._state.reset()
00315 
00316         if not self._empty_id:
00317             self._empty_id = self._tree_ctrl.AppendItem(self._tree_ctrl.GetRootItem(), "No data")
00318             self._tree_ctrl.SetItemImage(self._empty_id, self._image_dict[3])        
00319 
00320         self._is_stale = True
00321 
00322         self._have_message = False
00323 
00324         self._last_message_time = 0.0
00325         self._have_message = False
00326 
00327         self._set_initial_message_state()
00328 
00329     ##\brief processes new messages, updates tree control
00330     ##
00331     ## New messages clear tree under the any names in the message. Selected
00332     ## name, and expanded nodes will be expanded again after the tree clear.
00333     ## 
00334     def new_message(self, msg):
00335         self._tree_ctrl.Freeze()
00336         
00337         # Since we have message, remove empty item
00338         if not self._have_message:
00339             self._have_message = True
00340             self._tree_ctrl.Delete(self._empty_id)
00341             self._empty_id = None
00342             #wx.CallAfter(self._update_message_state)
00343             
00344         (added, removed, all) = self._state.update(msg)
00345         
00346         for a in added:
00347             self._create_tree_item(a)
00348             
00349         for r in removed:
00350             if (r.tree_id is not None):
00351                 self._tree_ctrl.Delete(r.tree_id)
00352             if (r.error_id is not None):
00353                 self._error_tree_ctrl.Delete(r.error_id)
00354             if (r.warning_id is not None):
00355                 self._warning_tree_ctrl.Delete(r.warning_id)
00356         
00357         # Update viewers
00358         for k,v in self._viewers.iteritems():
00359             if (all.has_key(k)):
00360                 v.set_status(all[k].status)
00361         
00362         self._update_status_images()
00363         
00364         self._update_error_tree()
00365         self._update_warning_tree()
00366         
00367         self._update_labels()
00368             
00369         self._tree_ctrl.Thaw()
00370         
00371     def _on_pause(self, paused):
00372         if (not paused and len(self._viewers) > 0):
00373             msgs = self._timeline.get_messages()
00374             states = []
00375             for msg in msgs:
00376                 state = State()
00377                 state.update(msg)
00378                 states.append(state)
00379         
00380         for v in self._viewers.itervalues():
00381             if (paused):
00382                 v.disable_timeline()
00383             else:
00384                 v.enable_timeline()    
00385                 for state in states:
00386                     all = state.get_items()
00387                     if (all.has_key(v.get_name())):
00388                         v.set_status(all[v.get_name()].status)
00389         
00390     def _update_error_tree(self):
00391         for item in self._state.get_items().itervalues():
00392             level = item.status.level
00393             if (level not in error_levels and item.error_id is not None):
00394                 self._error_tree_ctrl.Delete(item.error_id)
00395                 item.error_id = None
00396             elif (level in error_levels and item.error_id is None):
00397                 item.error_id = self._error_tree_ctrl.AppendItem(self._error_tree_ctrl.GetRootItem(), item.status.name)
00398                 self._error_tree_ctrl.SetItemImage(item.error_id, self._image_dict[level])
00399                 self._error_tree_ctrl.SetPyData(item.error_id, item)
00400                 
00401         self._error_tree_ctrl.SortChildren(self._error_tree_ctrl.GetRootItem())
00402                 
00403     def _update_warning_tree(self):
00404         for item in self._state.get_items().itervalues():
00405             level = item.status.level
00406             if (level != 1 and item.warning_id is not None):
00407                 self._warning_tree_ctrl.Delete(item.warning_id)
00408                 item.warning_id = None
00409             elif (level == 1 and item.warning_id is None):
00410                 item.warning_id = self._warning_tree_ctrl.AppendItem(self._warning_tree_ctrl.GetRootItem(), item.status.name)
00411                 self._warning_tree_ctrl.SetItemImage(item.warning_id, self._image_dict[level])
00412                 self._warning_tree_ctrl.SetPyData(item.warning_id, item)
00413                 
00414         self._warning_tree_ctrl.SortChildren(self._warning_tree_ctrl.GetRootItem())
00415         
00416     def _update_status_images(self):
00417         for item in self._state.get_items().itervalues():
00418             if (item.tree_id is not None):
00419                 level = item.status.level
00420                 
00421                 if (item.status.level != item.last_level):
00422                     self._tree_ctrl.SetItemImage(item.tree_id, self._image_dict[level])
00423                     item.last_level = level
00424     
00425     def _update_labels(self):
00426         for item in self._state.get_items().itervalues():
00427             children = self._state.get_descendants(item)
00428             errors = 0
00429             warnings = 0
00430             for child in children:
00431                 if (child.status.level == 2):
00432                     errors = errors + 1
00433                 elif (child.status.level == 1):
00434                     warnings = warnings + 1
00435             
00436             base_text = "%s : %s"%(get_nice_name(item.status.name), item.status.message)
00437             errwarn_text = "%s : %s"%(item.status.name, item.status.message)
00438             
00439             if (item.tree_id is not None):
00440                 text = base_text
00441                 if (errors > 0 or warnings > 0):
00442                     text = "(E: %s, W: %s) %s"%(errors, warnings, base_text)
00443                 self._tree_ctrl.SetItemText(item.tree_id, text)
00444             if (item.error_id is not None):
00445                 self._error_tree_ctrl.SetItemText(item.error_id, errwarn_text)
00446             if (item.warning_id is not None):
00447                 self._warning_tree_ctrl.SetItemText(item.warning_id, errwarn_text)
00448                 
00449           
00450     def _create_tree_item(self, item):
00451         # Find parent
00452         parent = self._state.get_parent(item)
00453         
00454         parent_id = self._tree_ctrl.GetRootItem()
00455         if (parent is not None):
00456             parent_id = parent.tree_id
00457         
00458         ## Add item to tree as short name
00459         short_name = get_nice_name(item.status.name)
00460         id = self._tree_ctrl.AppendItem(parent_id, short_name)
00461         item.tree_id = id
00462         self._tree_ctrl.SetPyData(id, item)
00463         self._tree_ctrl.SortChildren(parent_id)
00464 
00465     ##\brief Removes StatusViewerFrame from list to update
00466     ##\param name str : Status name to remove from dictionary
00467     def remove_viewer(self, name):
00468         if self._viewers.has_key(name):
00469             del self._viewers[name]
00470 
00471     def on_all_item_activate(self, event):
00472         self._on_item_activate(event, self._tree_ctrl)
00473         
00474     def on_error_item_activate(self, event):
00475         self._on_item_activate(event, self._error_tree_ctrl)
00476         
00477     def on_warning_item_activate(self, event):
00478         self._on_item_activate(event, self._warning_tree_ctrl)
00479         
00480     def _on_item_activate(self, event, tree_ctrl):
00481         id = event.GetItem()
00482         if id == None:
00483             event.Skip()
00484             return
00485 
00486         if tree_ctrl.ItemHasChildren(id):
00487             tree_ctrl.Expand(id)
00488 
00489         item = tree_ctrl.GetPyData(id)
00490         if not (item and item.status):
00491             event.Skip()
00492             return
00493 
00494         name = item.status.name
00495         
00496         if (self._viewers.has_key(name)):
00497             self._viewers[name].Raise()
00498         else:
00499             title = get_nice_name(name)
00500             
00501             ##\todo Move this viewer somewhere useful
00502             viewer = StatusViewerFrame(self._frame, name, self, title)
00503             viewer.SetSize(wx.Size(500, 600))
00504             viewer.Layout()
00505             viewer.Center()
00506             viewer.Show(True)
00507             viewer.Raise()
00508     
00509             self._viewers[name] = viewer
00510     
00511             if (self._timeline.is_paused() or self._rxbag):
00512                 viewer.disable_timeline()
00513                 viewer.set_status(item.status)
00514             else:
00515                 msgs = self._timeline.get_messages()
00516                 states = []
00517                 for msg in msgs:
00518                     state = State()
00519                     state.update(msg)
00520                     states.append(state)
00521                     
00522                 for state in states:
00523                     all = state.get_items()
00524                     if (all.has_key(item.status.name)):
00525                         viewer.set_status(all[item.status.name].status)
00526                         
00527             
00528 
00529     ##\brief Gets the state of the "top level" diagnostics
00530     ##
00531     ## Returns the highest value of any of the root tree items
00532     ##\return -1 = No diagnostics yet, 0 = OK, 1 = Warning, 2 = Error, 3 = All Stale
00533     def get_top_level_state(self):
00534         if (self._is_stale):
00535             return 3
00536         
00537         level = -1
00538         min_level = 255
00539 
00540         if len(self._state.get_items()) == 0:
00541             return level
00542 
00543         for item in self._state.get_items().itervalues():
00544             # Only look at "top level" items
00545             if self._state.get_parent(item) is not None:
00546                 continue
00547 
00548             if item.status.level > level:
00549                 level = item.status.level
00550             if item.status.level < min_level:
00551                 min_level = item.status.level
00552               
00553         # Top level is error if we have stale items, unless all stale
00554         if level > 2 and min_level <= 2:
00555             level = 2
00556 
00557         return level
00558 
00559     def get_color_for_message(self, msg):
00560         level = 0
00561         min_level = 255
00562         
00563         lookup = {}
00564         for status in msg.status:
00565             lookup[status.name] = status
00566             
00567         names = [status.name for status in msg.status]
00568         names = [name for name in names if len(get_parent_name(name)) == 0]
00569         for name in names:
00570             status = lookup[name]
00571             if (status.level > level):
00572                 level = status.level
00573             if (status.level < min_level):
00574                 min_level = status.level
00575 
00576         # Stale items should be reported as errors unless all stale
00577         if (level > 2 and min_level <= 2):
00578             level = 2
00579 
00580                 
00581         return color_dict[level]


robot_monitor
Author(s): Kevin Watts (watts@willowgarage.com), Josh Faust (jfaust@willowgarage.com)
autogenerated on Sat Dec 28 2013 16:53:42