$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 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 #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.""")) 00246 00247 ##\brief Sets robot monitor messages status in default start configuration 00248 def _set_initial_message_state(self): 00249 if self._rxbag: 00250 self._message_status_text.SetLabel("No message received") 00251 self._message_status_text.SetForegroundColour(color_dict[1]) 00252 self._tree_ctrl.SetBackgroundColour(wx.LIGHT_GREY) 00253 self._error_tree_ctrl.SetBackgroundColour(wx.LIGHT_GREY) 00254 self._warning_tree_ctrl.SetBackgroundColour(wx.LIGHT_GREY) 00255 else: 00256 self._message_status_text.SetLabel("No message received") 00257 self._message_status_text.SetForegroundColour(color_dict[2]) 00258 self._tree_ctrl.SetBackgroundColour(wx.LIGHT_GREY) 00259 self._error_tree_ctrl.SetBackgroundColour(wx.LIGHT_GREY) 00260 self._warning_tree_ctrl.SetBackgroundColour(wx.LIGHT_GREY) 00261 00262 ##\brief Updates status bar with status of diagnostics_agg topic 00263 def _update_message_state(self, event = None): 00264 if not self._have_message: 00265 self._set_initial_message_state() 00266 return 00267 if self._rxbag: 00268 self._message_status_text.SetLabel("Receiving rxbag messages") 00269 self._message_status_text.SetForegroundColour(color_dict[0]) 00270 self._tree_ctrl.SetBackgroundColour(wx.WHITE) 00271 self._error_tree_ctrl.SetBackgroundColour(wx.WHITE) 00272 self._warning_tree_ctrl.SetBackgroundColour(wx.WHITE) 00273 self._is_stale = False 00274 return # Return so we don't call get_time 00275 00276 current_time = rospy.get_time() 00277 time_diff = current_time - self._last_message_time 00278 if (time_diff > 10.0): 00279 self._message_status_text.SetLabel("Last message received %s seconds ago"%(int(time_diff))) 00280 self._message_status_text.SetForegroundColour(color_dict[2]) 00281 self._tree_ctrl.SetBackgroundColour(wx.LIGHT_GREY) 00282 self._error_tree_ctrl.SetBackgroundColour(wx.LIGHT_GREY) 00283 self._warning_tree_ctrl.SetBackgroundColour(wx.LIGHT_GREY) 00284 self._is_stale = True 00285 else: 00286 seconds_string = "seconds" 00287 if (int(time_diff) == 1): 00288 seconds_string = "second" 00289 self._message_status_text.SetLabel("Last message received %s %s ago"%(int(time_diff), seconds_string)) 00290 self._message_status_text.SetForegroundColour(color_dict[0]) 00291 self._tree_ctrl.SetBackgroundColour(wx.WHITE) 00292 self._error_tree_ctrl.SetBackgroundColour(wx.WHITE) 00293 self._warning_tree_ctrl.SetBackgroundColour(wx.WHITE) 00294 self._is_stale = False 00295 00296 00297 ## \brief Called whenever a new message is received by the timeline. Different from new_message in that it 00298 ## is called even if the timeline is paused, and only when a new message is received, not when the timeline 00299 ## is scrubbed 00300 def _on_new_message_received(self, msg): 00301 self._last_message_time = rospy.get_time() 00302 00303 ##\brief Clears messages at startup, or for rxbag plugin 00304 def reset_monitor(self): 00305 # Reset tree control 00306 if self._have_message: 00307 self._tree_ctrl.DeleteChildren(self._tree_ctrl.GetRootItem()) 00308 self._error_tree_ctrl.DeleteChildren(self._error_tree_ctrl.GetRootItem()) 00309 self._warning_tree_ctrl.DeleteChildren(self._warning_tree_ctrl.GetRootItem()) 00310 self._state.reset() 00311 00312 if not self._empty_id: 00313 self._empty_id = self._tree_ctrl.AppendItem(self._tree_ctrl.GetRootItem(), "No data") 00314 self._tree_ctrl.SetItemImage(self._empty_id, self._image_dict[3]) 00315 00316 self._is_stale = True 00317 00318 self._have_message = False 00319 00320 self._last_message_time = 0.0 00321 self._have_message = False 00322 00323 self._set_initial_message_state() 00324 00325 ##\brief processes new messages, updates tree control 00326 ## 00327 ## New messages clear tree under the any names in the message. Selected 00328 ## name, and expanded nodes will be expanded again after the tree clear. 00329 ## 00330 def new_message(self, msg): 00331 self._tree_ctrl.Freeze() 00332 00333 # Since we have message, remove empty item 00334 if not self._have_message: 00335 self._have_message = True 00336 self._tree_ctrl.Delete(self._empty_id) 00337 self._empty_id = None 00338 #wx.CallAfter(self._update_message_state) 00339 00340 (added, removed, all) = self._state.update(msg) 00341 00342 for a in added: 00343 self._create_tree_item(a) 00344 00345 for r in removed: 00346 if (r.tree_id is not None): 00347 self._tree_ctrl.Delete(r.tree_id) 00348 if (r.error_id is not None): 00349 self._error_tree_ctrl.Delete(r.error_id) 00350 if (r.warning_id is not None): 00351 self._warning_tree_ctrl.Delete(r.warning_id) 00352 00353 # Update viewers 00354 for k,v in self._viewers.iteritems(): 00355 if (all.has_key(k)): 00356 v.set_status(all[k].status) 00357 00358 self._update_status_images() 00359 00360 self._update_error_tree() 00361 self._update_warning_tree() 00362 00363 self._update_labels() 00364 00365 self._tree_ctrl.Thaw() 00366 00367 def _on_pause(self, paused): 00368 if (not paused and len(self._viewers) > 0): 00369 msgs = self._timeline.get_messages() 00370 states = [] 00371 for msg in msgs: 00372 state = State() 00373 state.update(msg) 00374 states.append(state) 00375 00376 for v in self._viewers.itervalues(): 00377 if (paused): 00378 v.disable_timeline() 00379 else: 00380 v.enable_timeline() 00381 for state in states: 00382 all = state.get_items() 00383 if (all.has_key(v.get_name())): 00384 v.set_status(all[v.get_name()].status) 00385 00386 def _update_error_tree(self): 00387 for item in self._state.get_items().itervalues(): 00388 if len(self._state.get_descendants(item)) == 0: 00389 level = item.status.level 00390 if (level not in error_levels and item.error_id is not None): 00391 self._error_tree_ctrl.Delete(item.error_id) 00392 item.error_id = None 00393 elif (level in error_levels and item.error_id is None): 00394 item.error_id = self._error_tree_ctrl.AppendItem(self._error_tree_ctrl.GetRootItem(), item.status.name) 00395 self._error_tree_ctrl.SetItemImage(item.error_id, self._image_dict[level]) 00396 self._error_tree_ctrl.SetPyData(item.error_id, item) 00397 00398 self._error_tree_ctrl.SortChildren(self._error_tree_ctrl.GetRootItem()) 00399 00400 def _update_warning_tree(self): 00401 for item in self._state.get_items().itervalues(): 00402 if len(self._state.get_descendants(item)) == 0: 00403 level = item.status.level 00404 if (level != 1 and item.warning_id is not None): 00405 self._warning_tree_ctrl.Delete(item.warning_id) 00406 item.warning_id = None 00407 elif (level == 1 and item.warning_id is None): 00408 item.warning_id = self._warning_tree_ctrl.AppendItem(self._warning_tree_ctrl.GetRootItem(), item.status.name) 00409 self._warning_tree_ctrl.SetItemImage(item.warning_id, self._image_dict[level]) 00410 self._warning_tree_ctrl.SetPyData(item.warning_id, item) 00411 00412 self._warning_tree_ctrl.SortChildren(self._warning_tree_ctrl.GetRootItem()) 00413 00414 def _update_status_images(self): 00415 for item in self._state.get_items().itervalues(): 00416 if (item.tree_id is not None): 00417 level = item.status.level 00418 00419 if (item.status.level != item.last_level): 00420 self._tree_ctrl.SetItemImage(item.tree_id, self._image_dict[level]) 00421 item.last_level = level 00422 00423 def _update_labels(self): 00424 for item in self._state.get_items().itervalues(): 00425 children = self._state.get_descendants(item) 00426 errors = 0 00427 warnings = 0 00428 for child in children: 00429 if (child.status.level == 2): 00430 errors = errors + 1 00431 elif (child.status.level == 1): 00432 warnings = warnings + 1 00433 00434 base_text = "%s : %s"%(get_nice_name(item.status.name), item.status.message) 00435 errwarn_text = "%s : %s"%(item.status.name, item.status.message) 00436 00437 if (item.tree_id is not None): 00438 text = base_text 00439 if (errors > 0 or warnings > 0): 00440 text = "(E: %s, W: %s) %s"%(errors, warnings, base_text) 00441 self._tree_ctrl.SetItemText(item.tree_id, text) 00442 if (item.error_id is not None): 00443 self._error_tree_ctrl.SetItemText(item.error_id, errwarn_text) 00444 if (item.warning_id is not None): 00445 self._warning_tree_ctrl.SetItemText(item.warning_id, errwarn_text) 00446 00447 00448 def _create_tree_item(self, item): 00449 # Find parent 00450 parent = self._state.get_parent(item) 00451 00452 parent_id = self._tree_ctrl.GetRootItem() 00453 if (parent is not None): 00454 parent_id = parent.tree_id 00455 00456 ## Add item to tree as short name 00457 short_name = get_nice_name(item.status.name) 00458 id = self._tree_ctrl.AppendItem(parent_id, short_name) 00459 item.tree_id = id 00460 self._tree_ctrl.SetPyData(id, item) 00461 self._tree_ctrl.SortChildren(parent_id) 00462 00463 ##\brief Removes StatusViewerFrame from list to update 00464 ##\param name str : Status name to remove from dictionary 00465 def remove_viewer(self, name): 00466 if self._viewers.has_key(name): 00467 del self._viewers[name] 00468 00469 def on_all_item_activate(self, event): 00470 self._on_item_activate(event, self._tree_ctrl) 00471 00472 def on_error_item_activate(self, event): 00473 self._on_item_activate(event, self._error_tree_ctrl) 00474 00475 def on_warning_item_activate(self, event): 00476 self._on_item_activate(event, self._warning_tree_ctrl) 00477 00478 def _on_item_activate(self, event, tree_ctrl): 00479 id = event.GetItem() 00480 if id == None: 00481 event.Skip() 00482 return 00483 00484 if tree_ctrl.ItemHasChildren(id): 00485 tree_ctrl.Expand(id) 00486 00487 item = tree_ctrl.GetPyData(id) 00488 if not (item and item.status): 00489 event.Skip() 00490 return 00491 00492 name = item.status.name 00493 00494 if (self._viewers.has_key(name)): 00495 self._viewers[name].Raise() 00496 else: 00497 title = get_nice_name(name) 00498 00499 ##\todo Move this viewer somewhere useful 00500 viewer = StatusViewerFrame(self._frame, name, self, title) 00501 viewer.SetSize(wx.Size(500, 600)) 00502 viewer.Layout() 00503 viewer.Center() 00504 viewer.Show(True) 00505 viewer.Raise() 00506 00507 self._viewers[name] = viewer 00508 00509 if (self._timeline.is_paused() or self._rxbag): 00510 viewer.disable_timeline() 00511 viewer.set_status(item.status) 00512 else: 00513 msgs = self._timeline.get_messages() 00514 states = [] 00515 for msg in msgs: 00516 state = State() 00517 state.update(msg) 00518 states.append(state) 00519 00520 for state in states: 00521 all = state.get_items() 00522 if (all.has_key(item.status.name)): 00523 viewer.set_status(all[item.status.name].status) 00524 00525 00526 00527 ##\brief Gets the state of the "top level" diagnostics 00528 ## 00529 ## Returns the highest value of any of the root tree items 00530 ##\return -1 = No diagnostics yet, 0 = OK, 1 = Warning, 2 = Error, 3 = All Stale 00531 def get_top_level_state(self): 00532 if (self._is_stale): 00533 return 3 00534 00535 level = -1 00536 min_level = 255 00537 00538 if len(self._state.get_items()) == 0: 00539 return level 00540 00541 for item in self._state.get_items().itervalues(): 00542 # Only look at "top level" items 00543 if self._state.get_parent(item) is not None: 00544 continue 00545 00546 if item.status.level > level: 00547 level = item.status.level 00548 if item.status.level < min_level: 00549 min_level = item.status.level 00550 00551 # Top level is error if we have stale items, unless all stale 00552 if level > 2 and min_level <= 2: 00553 level = 2 00554 00555 return level 00556 00557 def get_color_for_message(self, msg): 00558 level = 0 00559 min_level = 255 00560 00561 lookup = {} 00562 for status in msg.status: 00563 lookup[status.name] = status 00564 00565 names = [status.name for status in msg.status] 00566 names = [name for name in names if len(get_parent_name(name)) == 0] 00567 for name in names: 00568 status = lookup[name] 00569 if (status.level > level): 00570 level = status.level 00571 if (status.level < min_level): 00572 min_level = status.level 00573 00574 # Stale items should be reported as errors unless all stale 00575 if (level > 2 and min_level <= 2): 00576 level = 2 00577 00578 00579 return color_dict[level]