smach_graph.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (c) 2010, Willow Garage, Inc.
00004 # All rights reserved.
00005 # Copyright (c) 2013, Jonathan Bohren, The Johns Hopkins University
00006 # 
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions 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 copyright
00013 #       notice, this list of conditions and the following disclaimer in the
00014 #       documentation and/or other materials provided with the distribution.
00015 #   * Neither the name of the Willow Garage, Inc. nor the names of its
00016 #       contributors may be used to endorse or promote products derived from
00017 #       this software without specific prior written permission.
00018 # 
00019 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00026 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029 # POSSIBILITY OF SUCH DAMAGE.
00030 #
00031 # Author: Jonathan Bohren 
00032 
00033 import roslib; roslib.load_manifest('smach_viewer')
00034 import rospy
00035 
00036 from smach_msgs.msg import SmachContainerStatus,SmachContainerInitialStatusCmd,SmachContainerStructure
00037 
00038 import sys
00039 import os
00040 import threading
00041 import pickle
00042 import pprint
00043 import copy
00044 import StringIO
00045 import colorsys
00046 
00047 import wxversion
00048 wxversion.select("2.8")
00049 import wx
00050 import wx.richtext
00051 
00052 import textwrap
00053 
00054 import xdot
00055 import smach
00056 import smach_ros
00057 
00058 ### Helper Functions
00059 def graph_attr_string(attrs):
00060     """Generate an xdot graph attribute string."""
00061     attrs_strs = ['"'+str(k)+'"="'+str(v)+'"' for k,v in attrs.iteritems()]
00062     return ';\n'.join(attrs_strs)+';\n'
00063 
00064 def attr_string(attrs):
00065     """Generate an xdot node attribute string."""
00066     attrs_strs = ['"'+str(k)+'"="'+str(v)+'"' for k,v in attrs.iteritems()]
00067     return ' ['+(', '.join(attrs_strs))+']'
00068 
00069 def get_parent_path(path):
00070     """Get the parent path of an xdot node."""
00071     path_tokens = path.split('/')
00072     if len(path_tokens) > 2:
00073         parent_path = '/'.join(path_tokens[0:-1])
00074     else:
00075         parent_path = '/'.join(path_tokens[0:1])
00076     return parent_path
00077 
00078 def get_label(path):
00079     """Get the label of an xdot node."""
00080     path_tokens = path.split('/')
00081     return path_tokens[-1]
00082 
00083 def hex2t(color_str):
00084     """Convert a hexadecimal color strng into a color tuple."""
00085     color_tuple = [int(color_str[i:i+2],16)/255.0    for i in range(1,len(color_str),2)]
00086     return color_tuple
00087 
00088 class ContainerNode():
00089     """
00090     This class represents a given container in a running SMACH system. 
00091 
00092     Its primary use is to generate dotcode for a SMACH container. It has
00093     methods for responding to structure and status messages from a SMACH
00094     introspection server, as well as methods for updating the styles of a 
00095     graph once it's been drawn.
00096     """
00097     def __init__(self, server_name, msg):
00098         # Store path info
00099         self._server_name = server_name
00100         self._path = msg.path
00101         splitpath = msg.path.split('/')
00102         self._label = splitpath[-1]
00103         self._dir = '/'.join(splitpath[0:-1])
00104 
00105         self._children = msg.children
00106         self._internal_outcomes = msg.internal_outcomes
00107         self._outcomes_from = msg.outcomes_from
00108         self._outcomes_to = msg.outcomes_to
00109 
00110         self._container_outcomes = msg.container_outcomes
00111 
00112         # Status
00113         self._initial_states = []
00114         self._active_states = []
00115         self._last_active_states = []
00116         self._local_data = smach.UserData()
00117         self._info = ''
00118 
00119     def update_structure(self, msg):
00120         """Update the structure of this container from a given message. Return True if anything changes."""
00121         needs_update = False
00122 
00123         if self._children != msg.children\
00124                 or self._internal_outcomes != msg.internal_outcomes\
00125                 or self._outcomes_from != msg.outcomes_from\
00126                 or self._outcomes_to != msg.outcomes_to\
00127                 or self._container_outcomes != msg.container_outcomes:
00128             needs_update = True
00129 
00130         if needs_update:
00131             self._children = msg.children
00132             self._internal_outcomes = msg.internal_outcomes
00133             self._outcomes_from = msg.outcomes_from
00134             self._outcomes_to = msg.outcomes_to
00135 
00136             self._container_outcomes = msg.container_outcomes
00137 
00138         return needs_update
00139 
00140     def update_status(self, msg):
00141         """Update the known userdata and active state set and return True if the graph needs to be redrawn."""
00142 
00143         # Initialize the return value
00144         needs_update = False
00145 
00146         # Check if the initial states or active states have changed
00147         if set(msg.initial_states) != set(self._initial_states):
00148             self._structure_changed = True
00149             needs_update = True
00150         if set(msg.active_states) != set(self._active_states):
00151             needs_update = True
00152 
00153         # Store the initial and active states
00154         self._initial_states = msg.initial_states
00155         self._last_active_states = self._active_states
00156         self._active_states = msg.active_states
00157 
00158         # Unpack the user data
00159         while not rospy.is_shutdown():
00160             try:
00161                 self._local_data._data = pickle.loads(msg.local_data)
00162                 break
00163             except ImportError as ie:
00164                 # This will only happen once for each package
00165                 modulename = ie.args[0][16:]
00166                 packagename = modulename[0:modulename.find('.')]
00167                 roslib.load_manifest(packagename)
00168                 self._local_data._data = pickle.loads(msg.local_data)
00169 
00170         # Store the info string
00171         self._info = msg.info
00172 
00173         return needs_update
00174 
00175     def get_dotcode(self, selected_paths, closed_paths, depth, max_depth, containers, show_all, label_wrapper, attrs={}):
00176         """Generate the dotcode representing this container.
00177         
00178         @param selected_paths: The paths to nodes that are selected
00179         @closed paths: The paths that shouldn't be expanded
00180         @param depth: The depth to start traversing the tree
00181         @param max_depth: The depth to which we should traverse the tree
00182         @param containers: A dict of containers keyed by their paths
00183         @param show_all: True if implicit transitions should be shown
00184         @param label_wrapper: A text wrapper for wrapping element names
00185         @param attrs: A dict of dotcode attributes for this cluster
00186         """
00187 
00188         dotstr = 'subgraph "cluster_%s" {\n' % (self._path)
00189         if depth == 0:
00190             #attrs['style'] = 'filled,rounded'
00191             attrs['color'] = '#00000000'
00192             attrs['fillcolor'] = '#0000000F'
00193         #attrs['rank'] = 'max'
00194 
00195         #,'succeeded','aborted','preempted'attrs['label'] = self._label
00196         dotstr += graph_attr_string(attrs)
00197 
00198         # Add start/terimate target
00199         proxy_attrs = {
00200                 'URL':self._path,
00201                 'shape':'plaintext',
00202                 'color':'gray',
00203                 'fontsize':'18',
00204                 'fontweight':'18',
00205                 'rank':'min',
00206                 'height':'0.01'}
00207         proxy_attrs['label'] = '\\n'.join(label_wrapper.wrap(self._label))
00208         dotstr += '"%s" %s;\n' % (
00209                 '/'.join([self._path,'__proxy__']),
00210                 attr_string(proxy_attrs))
00211 
00212         # Check if we should expand this container
00213         if max_depth == -1 or depth <= max_depth:
00214             # Add container outcomes
00215             dotstr += 'subgraph "cluster_%s" {\n' % '/'.join([self._path,'__outcomes__'])
00216             outcomes_attrs = {
00217                     'style':'rounded,filled',
00218                     'rank':'sink',
00219                     'color':'#FFFFFFFF',#'#871C34',
00220                     'fillcolor':'#FFFFFF00'#'#FE464f3F'#'#DB889A'
00221                     }
00222             dotstr += graph_attr_string(outcomes_attrs)
00223 
00224             for outcome_label in self._container_outcomes:
00225                 outcome_path = ':'.join([self._path,outcome_label])
00226                 outcome_attrs = {
00227                         'shape':'box',
00228                         'height':'0.3',
00229                         'style':'filled,rounded',
00230                         'fontsize':'12',
00231                         'fillcolor':'#FE464f',#'#EDC2CC',
00232                         'color':'#780006',#'#EBAEBB',
00233                         'fontcolor':'#780006',#'#EBAEBB',
00234                         'label':'\\n'.join(label_wrapper.wrap(outcome_label)),
00235                         'URL':':'.join([self._path,outcome_label])
00236                         }
00237                 dotstr += '"%s" %s;\n' % (outcome_path,attr_string(outcome_attrs))
00238             dotstr += "}\n"
00239 
00240             # Iterate over children
00241             for child_label in self._children:
00242                 child_attrs = {
00243                         'style':'filled,setlinewidth(2)',
00244                         'color':'#000000FF',
00245                         'fillcolor':'#FFFFFF00'
00246                         }
00247 
00248                 child_path = '/'.join([self._path,child_label])
00249                 # Generate dotcode for children
00250                 if child_path in containers:
00251                     child_attrs['style'] += ',rounded'
00252 
00253                     dotstr += containers[child_path].get_dotcode(
00254                             selected_paths,
00255                             closed_paths,
00256                             depth+1, max_depth,
00257                             containers,
00258                             show_all,
00259                             label_wrapper,
00260                             child_attrs)
00261                 else:
00262                     child_attrs['label'] = '\\n'.join(label_wrapper.wrap(child_label))
00263                     child_attrs['URL'] = child_path
00264                     dotstr += '"%s" %s;\n' % (child_path, attr_string(child_attrs))
00265 
00266             # Iterate over edges
00267             internal_edges = zip(
00268                     self._internal_outcomes,
00269                     self._outcomes_from,
00270                     self._outcomes_to)
00271 
00272             # Add edge from container label to initial state
00273             internal_edges += [('','__proxy__',initial_child) for initial_child in self._initial_states]
00274 
00275             has_explicit_transitions = []
00276             for (outcome_label,from_label,to_label) in internal_edges:
00277                 if to_label != 'None' or outcome_label == to_label:
00278                     has_explicit_transitions.append(from_label)
00279 
00280             # Draw internal edges
00281             for (outcome_label,from_label,to_label) in internal_edges:
00282 
00283                 from_path = '/'.join([self._path, from_label])
00284 
00285                 if show_all \
00286                         or to_label != 'None'\
00287                         or from_label not in has_explicit_transitions \
00288                         or (outcome_label == from_label) \
00289                         or from_path in containers:
00290                     # Set the implicit target of this outcome
00291                     if to_label == 'None':
00292                         to_label = outcome_label
00293 
00294                     to_path = '/'.join([self._path, to_label])
00295 
00296                     edge_attrs = {
00297                             'URL':':'.join([from_path,outcome_label,to_path]),
00298                             'fontsize':'12',
00299                             'label':'\\n'.join(label_wrapper.wrap(outcome_label))}
00300                     edge_attrs['style'] = 'setlinewidth(2)'
00301 
00302                     # Hide implicit
00303                     #if not show_all and to_label == outcome_label:
00304                     #    edge_attrs['style'] += ',invis'
00305 
00306                     from_key = '"%s"' % from_path
00307                     if from_path in containers:
00308                         if max_depth == -1 or depth+1 <= max_depth:
00309                             from_key = '"%s:%s"' % ( from_path, outcome_label)
00310                         else:
00311                             edge_attrs['ltail'] = 'cluster_'+from_path
00312                             from_path = '/'.join([from_path,'__proxy__'])
00313                             from_key = '"%s"' % ( from_path )
00314 
00315                     to_key = ''
00316                     if to_label in self._container_outcomes:
00317                         to_key = '"%s:%s"' % (self._path,to_label)
00318                         edge_attrs['color'] = '#00000055'# '#780006'
00319                     else:
00320                         if to_path in containers:
00321                             edge_attrs['lhead'] = 'cluster_'+to_path
00322                             to_path = '/'.join([to_path,'__proxy__'])
00323                         to_key = '"%s"' % to_path
00324 
00325                     dotstr += '%s -> %s %s;\n' % (
00326                             from_key, to_key, attr_string(edge_attrs))
00327 
00328         dotstr += '}\n'
00329         return dotstr
00330 
00331     def set_styles(self, selected_paths, depth, max_depth, items, subgraph_shapes, containers):
00332         """Update the styles for a list of containers without regenerating the dotcode.
00333 
00334         This function is called recursively to update an entire tree.
00335         
00336         @param selected_paths: A list of paths to nodes that are currently selected.
00337         @param depth: The depth to start traversing the tree
00338         @param max_depth: The depth to traverse into the tree
00339         @param items: A dict of all the graph items, keyed by url
00340         @param subgraph_shapes: A dictionary of shapes from the rendering engine
00341         @param containers: A dict of all the containers
00342         """
00343 
00344         # Color root container
00345         """
00346         if depth == 0:
00347             container_shapes = subgraph_shapes['cluster_'+self._path]
00348             container_color = (0,0,0,0)
00349             container_fillcolor = (0,0,0,0)
00350 
00351             for shape in container_shapes:
00352                 shape.pen.color = container_color
00353                 shape.pen.fillcolor = container_fillcolor
00354                 """
00355 
00356         # Color shapes for outcomes
00357 
00358         # Color children
00359         if max_depth == -1 or depth <= max_depth:
00360             # Iterate over children
00361             for child_label in self._children:
00362                 child_path = '/'.join([self._path,child_label])
00363 
00364                 child_color = [0.5,0.5,0.5,1]
00365                 child_fillcolor = [1,1,1,1]
00366                 child_linewidth = 2
00367 
00368                 active_color = hex2t('#5C7600FF')
00369                 active_fillcolor = hex2t('#C0F700FF')
00370 
00371                 initial_color = hex2t('#000000FF')
00372                 initial_fillcolor = hex2t('#FFFFFFFF')
00373 
00374                 if child_label in self._active_states:
00375                     # Check if the child is active
00376                     child_color = active_color
00377                     child_fillcolor = active_fillcolor
00378                     child_linewidth = 5
00379                 elif child_label in self._initial_states:
00380                     # Initial style
00381                     #child_fillcolor = initial_fillcolor
00382                     child_color = initial_color
00383                     child_linewidth = 2
00384 
00385                 # Check if the child is selected
00386                 if child_path in selected_paths:
00387                     child_color = hex2t('#FB000DFF')
00388 
00389                 # Generate dotcode for child containers 
00390                 if child_path in containers:
00391                     subgraph_id = 'cluster_'+child_path
00392                     if subgraph_id in subgraph_shapes:
00393                         if child_label in self._active_states:
00394                             child_fillcolor[3] = 0.25
00395                         elif 0 and child_label in self._initial_states:
00396                             child_fillcolor[3] = 0.25
00397                         else:
00398                             if max_depth > 0:
00399                                 v = 1.0-0.25*((depth+1)/float(max_depth))
00400                             else:
00401                                 v = 0.85
00402                             child_fillcolor = [v,v,v,1.0]
00403 
00404                         
00405                         for shape in subgraph_shapes['cluster_'+child_path]:
00406                             pen = shape.pen
00407                             if len(pen.color) > 3:
00408                                 pen_color_opacity = pen.color[3]
00409                                 if pen_color_opacity < 0.01:
00410                                     pen_color_opacity = 0
00411                             else:
00412                                 pen_color_opacity = 0.5
00413                             shape.pen.color = child_color[0:3]+[pen_color_opacity]
00414                             shape.pen.fillcolor = [child_fillcolor[i] for i in range(min(3,len(pen.fillcolor)))]
00415                             shape.pen.linewidth = child_linewidth
00416 
00417                         # Recurse on this child
00418                         containers[child_path].set_styles(
00419                                 selected_paths,
00420                                 depth+1, max_depth,
00421                                 items,
00422                                 subgraph_shapes,
00423                                 containers)
00424                 else:
00425                     if child_path in items:
00426                         for shape in items[child_path].shapes:
00427                             if not isinstance(shape,xdot.xdot.TextShape):
00428                                 shape.pen.color = child_color
00429                                 shape.pen.fillcolor = child_fillcolor
00430                                 shape.pen.linewidth = child_linewidth
00431                     else:
00432                         #print child_path+" NOT IN "+str(items.keys())
00433                         pass
00434 
00435 class SmachViewerFrame(wx.Frame):
00436     """
00437     This class provides a GUI application for viewing SMACH plans.
00438     """
00439     def __init__(self):
00440         wx.Frame.__init__(self, None, -1, "Smach Viewer", size=(720,480))
00441 
00442         # Create graph
00443         self._containers = {}
00444         self._top_containers = {}
00445         self._update_cond = threading.Condition()
00446         self._needs_refresh = True
00447         
00448         vbox = wx.BoxSizer(wx.VERTICAL)
00449 
00450 
00451         # Create Splitter
00452         self.content_splitter = wx.SplitterWindow(self, -1,style = wx.SP_LIVE_UPDATE)
00453         self.content_splitter.SetMinimumPaneSize(24)
00454         self.content_splitter.SetSashGravity(0.85)
00455 
00456 
00457         # Create viewer pane
00458         viewer = wx.Panel(self.content_splitter,-1)
00459 
00460         # Create smach viewer 
00461         nb = wx.Notebook(viewer,-1,style=wx.NB_TOP | wx.WANTS_CHARS)
00462         viewer_box = wx.BoxSizer()
00463         viewer_box.Add(nb,1,wx.EXPAND | wx.ALL, 4)
00464         viewer.SetSizer(viewer_box)
00465 
00466         # Create graph view
00467         graph_view = wx.Panel(nb,-1)
00468         gv_vbox = wx.BoxSizer(wx.VERTICAL)
00469         graph_view.SetSizer(gv_vbox)
00470 
00471         # Construct toolbar
00472         toolbar = wx.ToolBar(graph_view, -1)
00473 
00474         toolbar.AddControl(wx.StaticText(toolbar,-1,"Path: "))
00475 
00476         # Path list
00477         self.path_combo = wx.ComboBox(toolbar, -1, style=wx.CB_DROPDOWN)
00478         self.path_combo .Bind(wx.EVT_COMBOBOX, self.set_path)
00479         self.path_combo.Append('/')
00480         self.path_combo.SetValue('/')
00481         toolbar.AddControl(self.path_combo)
00482 
00483         # Depth spinner
00484         self.depth_spinner = wx.SpinCtrl(toolbar, -1,
00485                 size=wx.Size(50,-1),
00486                 min=-1,
00487                 max=1337,
00488                 initial=-1)
00489         self.depth_spinner.Bind(wx.EVT_SPINCTRL,self.set_depth)
00490         self._max_depth = -1
00491         toolbar.AddControl(wx.StaticText(toolbar,-1,"    Depth: "))
00492         toolbar.AddControl(self.depth_spinner)
00493 
00494         # Label width spinner
00495         self.width_spinner = wx.SpinCtrl(toolbar, -1,
00496                 size=wx.Size(50,-1),
00497                 min=1,
00498                 max=1337,
00499                 initial=40)
00500         self.width_spinner.Bind(wx.EVT_SPINCTRL,self.set_label_width)
00501         self._label_wrapper = textwrap.TextWrapper(40,break_long_words=True)
00502         toolbar.AddControl(wx.StaticText(toolbar,-1,"    Label Width: "))
00503         toolbar.AddControl(self.width_spinner)
00504 
00505         # Implicit transition display
00506         toggle_all = wx.ToggleButton(toolbar,-1,'Show Implicit')
00507         toggle_all.Bind(wx.EVT_TOGGLEBUTTON, self.toggle_all_transitions)
00508         self._show_all_transitions = False
00509 
00510         toolbar.AddControl(wx.StaticText(toolbar,-1,"    "))
00511         toolbar.AddControl(toggle_all)
00512 
00513         toolbar.AddControl(wx.StaticText(toolbar,-1,"    "))
00514         toolbar.AddLabelTool(wx.ID_HELP, 'Help',
00515                 wx.ArtProvider.GetBitmap(wx.ART_HELP,wx.ART_OTHER,(16,16)) )
00516         toolbar.Realize()
00517 
00518 
00519         self.Bind(wx.EVT_TOOL, self.ShowControlsDialog, id=wx.ID_HELP)
00520 
00521         # Create dot graph widget
00522         self.widget = xdot.wxxdot.WxDotWindow(graph_view, -1)
00523 
00524         gv_vbox.Add(toolbar, 0, wx.EXPAND)
00525         gv_vbox.Add(self.widget, 1, wx.EXPAND)
00526 
00527         # Create tree view widget
00528         self.tree = wx.TreeCtrl(nb,-1,style=wx.TR_HAS_BUTTONS)
00529         nb.AddPage(graph_view,"Graph View")
00530         nb.AddPage(self.tree,"Tree View")
00531 
00532 
00533         # Create userdata widget
00534         borders = wx.LEFT | wx.RIGHT | wx.TOP
00535         border = 4
00536         self.ud_win = wx.ScrolledWindow(self.content_splitter, -1)
00537         self.ud_gs = wx.BoxSizer(wx.VERTICAL)
00538 
00539         self.ud_gs.Add(wx.StaticText(self.ud_win,-1,"Path:"),0, borders, border)
00540 
00541         self.path_input = wx.ComboBox(self.ud_win,-1,style=wx.CB_DROPDOWN)
00542         self.path_input.Bind(wx.EVT_COMBOBOX,self.selection_changed)
00543         self.ud_gs.Add(self.path_input,0,wx.EXPAND | borders, border)
00544 
00545 
00546         self.ud_gs.Add(wx.StaticText(self.ud_win,-1,"Userdata:"),0, borders, border)
00547 
00548         self.ud_txt = wx.TextCtrl(self.ud_win,-1,style=wx.TE_MULTILINE | wx.TE_READONLY)
00549         self.ud_gs.Add(self.ud_txt,1,wx.EXPAND | borders, border)
00550         
00551         # Add initial state button
00552         self.is_button = wx.Button(self.ud_win,-1,"Set as Initial State")
00553         self.is_button.Bind(wx.EVT_BUTTON, self.on_set_initial_state)
00554         self.is_button.Disable()
00555         self.ud_gs.Add(self.is_button,0,wx.EXPAND | wx.BOTTOM | borders, border)
00556 
00557         self.ud_win.SetSizer(self.ud_gs)
00558 
00559 
00560         # Set content splitter
00561         self.content_splitter.SplitVertically(viewer, self.ud_win, 512)
00562 
00563         # Add statusbar
00564         self.statusbar = wx.StatusBar(self,-1)
00565 
00566         # Add elements to sizer
00567         vbox.Add(self.content_splitter, 1, wx.EXPAND | wx.ALL)
00568         vbox.Add(self.statusbar, 0, wx.EXPAND)
00569 
00570         self.SetSizer(vbox)
00571         self.Center()
00572 
00573         # smach introspection client
00574         self._client = smach_ros.IntrospectionClient()
00575         self._containers= {}
00576         self._selected_paths = []
00577 
00578         # Message subscribers
00579         self._structure_subs = {}
00580         self._status_subs = {}
00581 
00582         self.Bind(wx.EVT_IDLE,self.OnIdle)
00583         self.Bind(wx.EVT_CLOSE,self.OnQuit)
00584 
00585         # Register mouse event callback
00586         self.widget.register_select_callback(self.select_cb)
00587         self._path = '/'
00588         self._needs_zoom = True
00589         self._structure_changed = True
00590 
00591         # Start a thread in the background to update the server list
00592         self._keep_running = True
00593         self._server_list_thread = threading.Thread(target=self._update_server_list)
00594         self._server_list_thread.start()
00595 
00596         self._update_graph_thread = threading.Thread(target=self._update_graph)
00597         self._update_graph_thread.start()
00598         self._update_tree_thread = threading.Thread(target=self._update_tree)
00599         self._update_tree_thread.start()
00600 
00601     def OnQuit(self,event):
00602         """Quit Event: kill threads and wait for join."""
00603         with self._update_cond:
00604             self._keep_running = False
00605             self._update_cond.notify_all()
00606 
00607         self._server_list_thread.join()
00608         self._update_graph_thread.join()
00609         self._update_tree_thread.join()
00610         
00611         event.Skip()
00612 
00613     def update_graph(self):
00614         """Notify all that the graph needs to be updated."""
00615         with self._update_cond:
00616             self._update_cond.notify_all()
00617 
00618     def on_set_initial_state(self, event):
00619         """Event: Change the initial state of the server."""
00620         state_path = self._selected_paths[0]
00621         parent_path = get_parent_path(state_path)
00622         state = get_label(state_path)
00623 
00624         server_name = self._containers[parent_path]._server_name
00625         self._client.set_initial_state(server_name,parent_path,[state],timeout = rospy.Duration(60.0))
00626 
00627     def set_path(self, event):
00628         """Event: Change the viewable path and update the graph."""
00629         self._path = self.path_combo.GetValue()
00630         self._needs_zoom = True
00631         self.update_graph()
00632 
00633     def set_depth(self, event):
00634         """Event: Change the maximum depth and update the graph."""
00635         self._max_depth = self.depth_spinner.GetValue()
00636         self._needs_zoom = True
00637         self.update_graph()
00638 
00639     def set_label_width(self, event):
00640         """Event: Change the label wrapper width and update the graph."""
00641         self._label_wrapper.width = self.width_spinner.GetValue()
00642         self._needs_zoom = True
00643         self.update_graph()
00644 
00645     def toggle_all_transitions(self, event):
00646         """Event: Change whether automatic transitions are hidden and update the graph."""
00647         self._show_all_transitions = not self._show_all_transitions
00648         self._structure_changed = True
00649         self.update_graph()
00650 
00651     def select_cb(self, item, event):
00652         """Event: Click to select a graph node to display user data and update the graph."""
00653         self.statusbar.SetStatusText(item.url)
00654         # Left button-up
00655         if event.ButtonUp(wx.MOUSE_BTN_LEFT):
00656             # Store this item's url as the selected path
00657             self._selected_paths = [item.url]
00658             # Update the selection dropdown
00659             self.path_input.SetValue(item.url)
00660             wx.PostEvent(
00661                     self.path_input.GetEventHandler(),
00662                     wx.CommandEvent(wx.wxEVT_COMMAND_COMBOBOX_SELECTED,self.path_input.GetId()))
00663             self.update_graph()
00664 
00665     def selection_changed(self, event):
00666         """Event: Selection dropdown changed."""
00667         path_input_str = self.path_input.GetValue()
00668 
00669         # Check the path is non-zero length
00670         if len(path_input_str) > 0:
00671             # Split the path (state:outcome), and get the state path
00672             path = path_input_str.split(':')[0]
00673 
00674             # Get the container corresponding to this path, since userdata is
00675             # stored in the containers
00676             if path not in self._containers:
00677                 parent_path = get_parent_path(path)
00678             else:
00679                 parent_path = path
00680 
00681             if parent_path in self._containers:
00682                 # Enable the initial state button for the selection
00683                 self.is_button.Enable()
00684 
00685                 # Get the container
00686                 container = self._containers[parent_path]
00687 
00688                 # Store the scroll position and selection
00689                 pos = self.ud_txt.HitTestPos(wx.Point(0,0))
00690                 sel = self.ud_txt.GetSelection()
00691 
00692                 # Generate the userdata string
00693                 ud_str = ''
00694                 for (k,v) in container._local_data._data.iteritems():
00695                     ud_str += str(k)+": "
00696                     vstr = str(v)
00697                     # Add a line break if this is a multiline value
00698                     if vstr.find('\n') != -1:
00699                         ud_str += '\n'
00700                     ud_str+=vstr+'\n\n'
00701 
00702                 # Set the userdata string
00703                 self.ud_txt.SetValue(ud_str)
00704 
00705                 # Restore the scroll position and selection
00706                 self.ud_txt.ShowPosition(pos[1])
00707                 if sel != (0,0):
00708                     self.ud_txt.SetSelection(sel[0],sel[1])
00709             else:
00710                 # Disable the initial state button for this selection
00711                 self.is_button.Disable()
00712 
00713     def _structure_msg_update(self, msg, server_name):
00714         """Update the structure of the SMACH plan (re-generate the dotcode)."""
00715 
00716         # Just return if we're shutting down
00717         if not self._keep_running:
00718             return
00719 
00720         # Get the node path
00721         path = msg.path
00722         pathsplit = path.split('/')
00723         parent_path = '/'.join(pathsplit[0:-1])
00724 
00725         rospy.logdebug("RECEIVED: "+path)
00726         rospy.logdebug("CONTAINERS: "+str(self._containers.keys()))
00727 
00728         # Initialize redraw flag
00729         needs_redraw = False
00730 
00731         if path in self._containers:
00732             rospy.logdebug("UPDATING: "+path)
00733 
00734             # Update the structure of this known container
00735             needs_redraw = self._containers[path].update_structure(msg)
00736         else: 
00737             rospy.logdebug("CONSTRUCTING: "+path)
00738 
00739             # Create a new container
00740             container = ContainerNode(server_name, msg)
00741             self._containers[path] = container
00742 
00743             # Store this as a top container if it has no parent
00744             if parent_path == '':
00745                 self._top_containers[path] = container
00746 
00747             # Append paths to selector
00748             self.path_combo.Append(path)
00749             self.path_input.Append(path)
00750 
00751             # We need to redraw thhe graph if this container's parent is already known
00752             if parent_path in self._containers:
00753                 needs_redraw= True
00754 
00755         # Update the graph if necessary
00756         if needs_redraw:
00757             with self._update_cond:
00758                 self._structure_changed = True
00759                 self._needs_zoom = True # TODO: Make it so you can disable this
00760                 self._update_cond.notify_all()
00761 
00762     def _status_msg_update(self, msg):
00763         """Process status messages."""
00764 
00765         # Check if we're in the process of shutting down
00766         if not self._keep_running:
00767             return
00768 
00769         # Get the path to the updating conainer
00770         path = msg.path
00771         rospy.logdebug("STATUS MSG: "+path)
00772 
00773         # Check if this is a known container
00774         if path in self._containers:
00775             # Get the container and check if the status update requires regeneration
00776             container = self._containers[path]
00777             if container.update_status(msg):
00778                 with self._update_cond:
00779                     self._update_cond.notify_all()
00780 
00781             # TODO: Is this necessary?
00782             path_input_str = self.path_input.GetValue()
00783             if path_input_str == path or get_parent_path(path_input_str) == path:
00784                 wx.PostEvent(
00785                         self.path_input.GetEventHandler(),
00786                         wx.CommandEvent(wx.wxEVT_COMMAND_COMBOBOX_SELECTED,self.path_input.GetId()))
00787 
00788     def _update_graph(self):
00789         """This thread continuously updates the graph when it changes.
00790 
00791         The graph gets updated in one of two ways:
00792 
00793           1: The structure of the SMACH plans has changed, or the display
00794           settings have been changed. In this case, the dotcode needs to be
00795           regenerated. 
00796 
00797           2: The status of the SMACH plans has changed. In this case, we only
00798           need to change the styles of the graph.
00799         """
00800         while self._keep_running and not rospy.is_shutdown():
00801             with self._update_cond:
00802                 # Wait for the update condition to be triggered
00803                 self._update_cond.wait()
00804 
00805                 # Get the containers to update
00806                 containers_to_update = {}
00807                 if self._path in self._containers:
00808                     # Some non-root path
00809                     containers_to_update = {self._path:self._containers[self._path]}
00810                 elif self._path == '/':
00811                     # Root path
00812                     containers_to_update = self._top_containers
00813 
00814                 # Check if we need to re-generate the dotcode (if the structure changed)
00815                 # TODO: needs_zoom is a misnomer
00816                 if self._structure_changed or self._needs_zoom:
00817                     dotstr = "digraph {\n\t"
00818                     dotstr += ';'.join([
00819                         "compound=true",
00820                         "outputmode=nodesfirst",
00821                         "labeljust=l",
00822                         "nodesep=0.5",
00823                         "minlen=2",
00824                         "mclimit=5",
00825                         "clusterrank=local",
00826                         "ranksep=0.75",
00827                         # "remincross=true",
00828                         # "rank=sink",
00829                         "ordering=\"\"",
00830                         ])
00831                     dotstr += ";\n"
00832 
00833                     # Generate the rest of the graph
00834                     # TODO: Only re-generate dotcode for containers that have changed
00835                     for path,tc in containers_to_update.iteritems():
00836                         dotstr += tc.get_dotcode(
00837                                 self._selected_paths,[],
00838                                 0,self._max_depth,
00839                                 self._containers,
00840                                 self._show_all_transitions,
00841                                 self._label_wrapper)
00842                     else:
00843                         dotstr += '"__empty__" [label="Path not available.", shape="plaintext"]'
00844 
00845                     dotstr += '\n}\n'
00846 
00847                     # Set the dotcode to the new dotcode, reset the flags
00848                     self.set_dotcode(dotstr,zoom=False)
00849                     self._structure_changed = False
00850 
00851                 # Update the styles for the graph if there are any updates
00852                 for path,tc in containers_to_update.iteritems():
00853                     tc.set_styles(
00854                             self._selected_paths,
00855                             0,self._max_depth,
00856                             self.widget.items_by_url,
00857                             self.widget.subgraph_shapes,
00858                             self._containers)
00859 
00860                 # Redraw
00861                 self.widget.Refresh()
00862 
00863     def set_dotcode(self, dotcode, zoom=True):
00864         """Set the xdot view's dotcode and refresh the display."""
00865         # Set the new dotcode
00866         if self.widget.set_dotcode(dotcode, None):
00867             self.SetTitle('Smach Viewer')
00868             # Re-zoom if necessary
00869             if zoom or self._needs_zoom:
00870                 self.widget.zoom_to_fit()
00871                 self._needs_zoom = False
00872             # Set the refresh flag
00873             self._needs_refresh = True
00874             wx.PostEvent(self.GetEventHandler(), wx.IdleEvent())
00875 
00876     def _update_tree(self):
00877         """Update the tree view."""
00878         while self._keep_running and not rospy.is_shutdown():
00879             with self._update_cond:
00880                 self._update_cond.wait()
00881                 self.tree.DeleteAllItems()
00882                 self._tree_nodes = {}
00883                 for path,tc in self._top_containers.iteritems():
00884                     self.add_to_tree(path, None)
00885 
00886     def add_to_tree(self, path, parent):
00887         """Add a path to the tree view."""
00888         if parent is None:
00889             container = self.tree.AddRoot(get_label(path))
00890         else:
00891             container = self.tree.AppendItem(parent,get_label(path))
00892 
00893         # Add children to tree
00894         for label in self._containers[path]._children:
00895             child_path = '/'.join([path,label])
00896             if child_path in self._containers.keys():
00897                 self.add_to_tree(child_path, container)
00898             else:
00899                 self.tree.AppendItem(container,label)
00900 
00901     def append_tree(self, container, parent = None):
00902         """Append an item to the tree view."""
00903         if not parent:
00904             node = self.tree.AddRoot(container._label)
00905             for child_label in container._children:
00906                 self.tree.AppendItem(node,child_label)
00907 
00908     def OnIdle(self, event):
00909         """Event: On Idle, refresh the display if necessary, then un-set the flag."""
00910         if self._needs_refresh:
00911             self.Refresh()
00912             # Re-populate path combo
00913             self._needs_refresh = False
00914 
00915     def _update_server_list(self):
00916         """Update the list of known SMACH introspection servers."""
00917         while self._keep_running:
00918             # Update the server list
00919             server_names = self._client.get_servers()
00920             new_server_names = [sn for sn in server_names if sn not in self._status_subs]
00921 
00922             # Create subscribers for new servers
00923             for server_name in new_server_names:
00924                 self._structure_subs[server_name] = rospy.Subscriber(
00925                         server_name+smach_ros.introspection.STRUCTURE_TOPIC,
00926                         SmachContainerStructure,
00927                         callback = self._structure_msg_update,
00928                         callback_args = server_name,
00929                         queue_size=50)
00930 
00931                 self._status_subs[server_name] = rospy.Subscriber(
00932                         server_name+smach_ros.introspection.STATUS_TOPIC,
00933                         SmachContainerStatus,
00934                         callback = self._status_msg_update,
00935                         queue_size=50)
00936 
00937             # This doesn't need to happen very often
00938             rospy.sleep(1.0)
00939             
00940             
00941             #self.server_combo.AppendItems([s for s in self._servers if s not in current_servers])
00942 
00943             # Grab the first server
00944             #current_value = self.server_combo.GetValue()
00945             #if current_value == '' and len(self._servers) > 0:
00946             #    self.server_combo.SetStringSelection(self._servers[0])
00947             #    self.set_server(self._servers[0])
00948 
00949     def ShowControlsDialog(self,event):
00950         dial = wx.MessageDialog(None,
00951                 "Pan: Arrow Keys\nZoom: PageUp / PageDown\nZoom To Fit: F\nRefresh: R",
00952                 'Keyboard Controls', wx.OK)
00953         dial.ShowModal()
00954 
00955     def OnExit(self, event):
00956         pass
00957 
00958     def set_filter(self, filter):
00959         self.widget.set_filter(filter)
00960 
00961 def main():
00962     app = wx.App()
00963 
00964     frame = SmachViewerFrame()
00965     frame.set_filter('dot')
00966 
00967     frame.Show()
00968 
00969     app.MainLoop()
00970 
00971 if __name__ == '__main__':
00972     rospy.init_node('smach_viewer',anonymous=False, disable_signals=True,log_level=rospy.INFO)
00973 
00974     main()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


rqt_smach
Author(s): Jonathan Bohren
autogenerated on Fri Jun 28 2013 11:08:56