smach_viewer.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Copyright (c) 2010, Willow Garage, Inc.
4 # All rights reserved.
5 # Copyright (c) 2013, Jonathan Bohren, The Johns Hopkins University
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions are met:
9 #
10 # * Redistributions of source code must retain the above copyright
11 # notice, this list of conditions and the following disclaimer.
12 # * Redistributions in binary form must reproduce the above copyright
13 # notice, this list of conditions and the following disclaimer in the
14 # documentation and/or other materials provided with the distribution.
15 # * Neither the name of the Willow Garage, Inc. nor the names of its
16 # contributors may be used to endorse or promote products derived from
17 # this software without specific prior written permission.
18 #
19 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
23 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29 # POSSIBILITY OF SUCH DAMAGE.
30 #
31 # Author: Jonathan Bohren
32 
33 import rospy
34 import rospkg
35 
36 from smach_msgs.msg import SmachContainerStatus,SmachContainerInitialStatusCmd,SmachContainerStructure
37 
38 import sys
39 import os
40 import threading
41 import pickle
42 import pprint
43 import copy
44 import StringIO
45 import colorsys
46 import time
47 
48 import wxversion
49 if wxversion.checkInstalled("2.8"):
50  wxversion.select("2.8")
51 else:
52  print("wxversion 2.8 is not installed, installed versions are {}".format(wxversion.getInstalled()))
53 import wx
54 import wx.richtext
55 
56 import textwrap
57 
58 ## this import system (or ros-released) xdot
59 # import xdot
60 ## need to import currnt package, but not to load this file
61 # http://stackoverflow.com/questions/6031584/importing-from-builtin-library-when-module-with-same-name-exists
62 def import_non_local(name, custom_name=None):
63  import imp, sys
64 
65  custom_name = custom_name or name
66 
67  path = filter(lambda x: x != os.path.dirname(os.path.abspath(__file__)), sys.path)
68  f, pathname, desc = imp.find_module(name, path)
69 
70  module = imp.load_module(custom_name, f, pathname, desc)
71  if f:
72  f.close()
73 
74  return module
75 
76 smach_viewer = import_non_local('smach_viewer')
77 from smach_viewer import xdot
78 
79 import smach
80 import smach_ros
81 
82 ### Helper Functions
83 def graph_attr_string(attrs):
84  """Generate an xdot graph attribute string."""
85  attrs_strs = ['"'+str(k)+'"="'+str(v)+'"' for k,v in attrs.iteritems()]
86  return ';\n'.join(attrs_strs)+';\n'
87 
88 def attr_string(attrs):
89  """Generate an xdot node attribute string."""
90  attrs_strs = ['"'+str(k)+'"="'+str(v)+'"' for k,v in attrs.iteritems()]
91  return ' ['+(', '.join(attrs_strs))+']'
92 
93 def get_parent_path(path):
94  """Get the parent path of an xdot node."""
95  path_tokens = path.split('/')
96  if len(path_tokens) > 2:
97  parent_path = '/'.join(path_tokens[0:-1])
98  else:
99  parent_path = '/'.join(path_tokens[0:1])
100  return parent_path
101 
102 def get_label(path):
103  """Get the label of an xdot node."""
104  path_tokens = path.split('/')
105  return path_tokens[-1]
106 
107 def hex2t(color_str):
108  """Convert a hexadecimal color strng into a color tuple."""
109  color_tuple = [int(color_str[i:i+2],16)/255.0 for i in range(1,len(color_str),2)]
110  return color_tuple
111 
113  """
114  This class represents a given container in a running SMACH system.
115 
116  Its primary use is to generate dotcode for a SMACH container. It has
117  methods for responding to structure and status messages from a SMACH
118  introspection server, as well as methods for updating the styles of a
119  graph once it's been drawn.
120  """
121  def __init__(self, server_name, msg):
122  # Store path info
123  self._server_name = server_name
124  self._path = msg.path
125  splitpath = msg.path.split('/')
126  self._label = splitpath[-1]
127  self._dir = '/'.join(splitpath[0:-1])
128 
129  self._children = msg.children
130  self._internal_outcomes = msg.internal_outcomes
131  self._outcomes_from = msg.outcomes_from
132  self._outcomes_to = msg.outcomes_to
133 
134  self._container_outcomes = msg.container_outcomes
135 
136  # Status
137  self._initial_states = []
138  self._active_states = []
140  self._local_data = smach.UserData()
141  self._info = ''
142 
143  def update_structure(self, msg):
144  """Update the structure of this container from a given message. Return True if anything changes."""
145  needs_update = False
146 
147  if self._children != msg.children\
148  or self._internal_outcomes != msg.internal_outcomes\
149  or self._outcomes_from != msg.outcomes_from\
150  or self._outcomes_to != msg.outcomes_to\
151  or self._container_outcomes != msg.container_outcomes:
152  needs_update = True
153 
154  if needs_update:
155  self._children = msg.children
156  self._internal_outcomes = msg.internal_outcomes
157  self._outcomes_from = msg.outcomes_from
158  self._outcomes_to = msg.outcomes_to
159 
160  self._container_outcomes = msg.container_outcomes
161 
162  return needs_update
163 
164  def update_status(self, msg):
165  """Update the known userdata and active state set and return True if the graph needs to be redrawn."""
166 
167  # Initialize the return value
168  needs_update = False
169 
170  # Check if the initial states or active states have changed
171  if set(msg.initial_states) != set(self._initial_states):
172  self._structure_changed = True
173  needs_update = True
174  if set(msg.active_states) != set(self._active_states):
175  needs_update = True
176 
177  # Store the initial and active states
178  self._initial_states = msg.initial_states
180  self._active_states = msg.active_states
181 
182  # Unpack the user data
183  while not rospy.is_shutdown():
184  try:
185  self._local_data._data = pickle.loads(msg.local_data)
186  break
187  except ImportError as ie:
188  # This will only happen once for each package
189  modulename = ie.args[0][16:]
190  packagename = modulename[0:modulename.find('.')]
191  roslib.load_manifest(packagename)
192  self._local_data._data = pickle.loads(msg.local_data)
193 
194  # Store the info string
195  self._info = msg.info
196 
197  return needs_update
198 
199  def get_dotcode(self, selected_paths, closed_paths, depth, max_depth, containers, show_all, label_wrapper, attrs={}):
200  """Generate the dotcode representing this container.
201 
202  @param selected_paths: The paths to nodes that are selected
203  @closed paths: The paths that shouldn't be expanded
204  @param depth: The depth to start traversing the tree
205  @param max_depth: The depth to which we should traverse the tree
206  @param containers: A dict of containers keyed by their paths
207  @param show_all: True if implicit transitions should be shown
208  @param label_wrapper: A text wrapper for wrapping element names
209  @param attrs: A dict of dotcode attributes for this cluster
210  """
211 
212  dotstr = 'subgraph "cluster_%s" {\n' % (self._path)
213  if depth == 0:
214  #attrs['style'] = 'filled,rounded'
215  attrs['color'] = '#00000000'
216  attrs['fillcolor'] = '#0000000F'
217  #attrs['rank'] = 'max'
218 
219  #,'succeeded','aborted','preempted'attrs['label'] = self._label
220  dotstr += graph_attr_string(attrs)
221 
222  # Add start/terimate target
223  proxy_attrs = {
224  'URL':self._path,
225  'shape':'plaintext',
226  'color':'gray',
227  'fontsize':'18',
228  'fontweight':'18',
229  'rank':'min',
230  'height':'0.01'}
231  proxy_attrs['label'] = '\\n'.join(label_wrapper.wrap(self._label))
232  dotstr += '"%s" %s;\n' % (
233  '/'.join([self._path,'__proxy__']),
234  attr_string(proxy_attrs))
235 
236  # Check if we should expand this container
237  if max_depth == -1 or depth <= max_depth:
238  # Add container outcomes
239  dotstr += 'subgraph "cluster_%s" {\n' % '/'.join([self._path,'__outcomes__'])
240  outcomes_attrs = {
241  'style':'rounded,filled',
242  'rank':'sink',
243  'color':'#FFFFFFFF',#'#871C34',
244  'fillcolor':'#FFFFFF00'#'#FE464f3F'#'#DB889A'
245  }
246  dotstr += graph_attr_string(outcomes_attrs)
247 
248  for outcome_label in self._container_outcomes:
249  outcome_path = ':'.join([self._path,outcome_label])
250  outcome_attrs = {
251  'shape':'box',
252  'height':'0.3',
253  'style':'filled,rounded',
254  'fontsize':'12',
255  'fillcolor':'#FE464f',#'#EDC2CC',
256  'color':'#780006',#'#EBAEBB',
257  'fontcolor':'#780006',#'#EBAEBB',
258  'label':'\\n'.join(label_wrapper.wrap(outcome_label)),
259  'URL':':'.join([self._path,outcome_label])
260  }
261  dotstr += '"%s" %s;\n' % (outcome_path,attr_string(outcome_attrs))
262  dotstr += "}\n"
263 
264  # Iterate over children
265  for child_label in self._children:
266  child_attrs = {
267  'style':'filled,setlinewidth(2)',
268  'color':'#000000FF',
269  'fillcolor':'#FFFFFF00'
270  }
271 
272  child_path = '/'.join([self._path,child_label])
273  # Generate dotcode for children
274  if child_path in containers:
275  child_attrs['style'] += ',rounded'
276 
277  dotstr += containers[child_path].get_dotcode(
278  selected_paths,
279  closed_paths,
280  depth+1, max_depth,
281  containers,
282  show_all,
283  label_wrapper,
284  child_attrs)
285  else:
286  child_attrs['label'] = '\\n'.join(label_wrapper.wrap(child_label))
287  child_attrs['URL'] = child_path
288  dotstr += '"%s" %s;\n' % (child_path, attr_string(child_attrs))
289 
290  # Iterate over edges
291  internal_edges = zip(
292  self._internal_outcomes,
293  self._outcomes_from,
294  self._outcomes_to)
295 
296  # Add edge from container label to initial state
297  internal_edges += [('','__proxy__',initial_child) for initial_child in self._initial_states]
298 
299  has_explicit_transitions = []
300  for (outcome_label,from_label,to_label) in internal_edges:
301  if to_label != 'None' or outcome_label == to_label:
302  has_explicit_transitions.append(from_label)
303 
304  # Draw internal edges
305  for (outcome_label,from_label,to_label) in internal_edges:
306 
307  from_path = '/'.join([self._path, from_label])
308 
309  if show_all \
310  or to_label != 'None'\
311  or from_label not in has_explicit_transitions \
312  or (outcome_label == from_label) \
313  or from_path in containers:
314  # Set the implicit target of this outcome
315  if to_label == 'None':
316  to_label = outcome_label
317 
318  to_path = '/'.join([self._path, to_label])
319 
320  edge_attrs = {
321  'URL':':'.join([from_path,outcome_label,to_path]),
322  'fontsize':'12',
323  'label':'\\n'.join(label_wrapper.wrap(outcome_label))}
324  edge_attrs['style'] = 'setlinewidth(2)'
325 
326  # Hide implicit
327  #if not show_all and to_label == outcome_label:
328  # edge_attrs['style'] += ',invis'
329 
330  from_key = '"%s"' % from_path
331  if from_path in containers:
332  if max_depth == -1 or depth+1 <= max_depth:
333  from_key = '"%s:%s"' % ( from_path, outcome_label)
334  else:
335  edge_attrs['ltail'] = 'cluster_'+from_path
336  from_path = '/'.join([from_path,'__proxy__'])
337  from_key = '"%s"' % ( from_path )
338 
339  to_key = ''
340  if to_label in self._container_outcomes:
341  to_key = '"%s:%s"' % (self._path,to_label)
342  edge_attrs['color'] = '#00000055'# '#780006'
343  else:
344  if to_path in containers:
345  edge_attrs['lhead'] = 'cluster_'+to_path
346  to_path = '/'.join([to_path,'__proxy__'])
347  to_key = '"%s"' % to_path
348 
349  dotstr += '%s -> %s %s;\n' % (
350  from_key, to_key, attr_string(edge_attrs))
351 
352  dotstr += '}\n'
353  return dotstr
354 
355  def set_styles(self, selected_paths, depth, max_depth, items, subgraph_shapes, containers):
356  """Update the styles for a list of containers without regenerating the dotcode.
357 
358  This function is called recursively to update an entire tree.
359 
360  @param selected_paths: A list of paths to nodes that are currently selected.
361  @param depth: The depth to start traversing the tree
362  @param max_depth: The depth to traverse into the tree
363  @param items: A dict of all the graph items, keyed by url
364  @param subgraph_shapes: A dictionary of shapes from the rendering engine
365  @param containers: A dict of all the containers
366  """
367 
368  # Color root container
369  """
370  if depth == 0:
371  container_shapes = subgraph_shapes['cluster_'+self._path]
372  container_color = (0,0,0,0)
373  container_fillcolor = (0,0,0,0)
374 
375  for shape in container_shapes:
376  shape.pen.color = container_color
377  shape.pen.fillcolor = container_fillcolor
378  """
379 
380  # Color shapes for outcomes
381 
382  # Color children
383  if max_depth == -1 or depth <= max_depth:
384  # Iterate over children
385  for child_label in self._children:
386  child_path = '/'.join([self._path,child_label])
387 
388  child_color = [0.5,0.5,0.5,1]
389  child_fillcolor = [1,1,1,1]
390  child_linewidth = 2
391 
392  active_color = hex2t('#5C7600FF')
393  active_fillcolor = hex2t('#C0F700FF')
394 
395  initial_color = hex2t('#000000FF')
396  initial_fillcolor = hex2t('#FFFFFFFF')
397 
398  if child_label in self._active_states:
399  # Check if the child is active
400  child_color = active_color
401  child_fillcolor = active_fillcolor
402  child_linewidth = 5
403  elif child_label in self._initial_states:
404  # Initial style
405  #child_fillcolor = initial_fillcolor
406  child_color = initial_color
407  child_linewidth = 2
408 
409  # Check if the child is selected
410  if child_path in selected_paths:
411  child_color = hex2t('#FB000DFF')
412 
413  # Generate dotcode for child containers
414  if child_path in containers:
415  subgraph_id = 'cluster_'+child_path
416  if subgraph_id in subgraph_shapes:
417  if child_label in self._active_states:
418  child_fillcolor[3] = 0.25
419  elif 0 and child_label in self._initial_states:
420  child_fillcolor[3] = 0.25
421  else:
422  if max_depth > 0:
423  v = 1.0-0.25*((depth+1)/float(max_depth))
424  else:
425  v = 0.85
426  child_fillcolor = [v,v,v,1.0]
427 
428 
429  for shape in subgraph_shapes['cluster_'+child_path]:
430  pen = shape.pen
431  if len(pen.color) > 3:
432  pen_color_opacity = pen.color[3]
433  if pen_color_opacity < 0.01:
434  pen_color_opacity = 0
435  else:
436  pen_color_opacity = 0.5
437  shape.pen.color = child_color[0:3]+[pen_color_opacity]
438  shape.pen.fillcolor = [child_fillcolor[i] for i in range(min(3,len(pen.fillcolor)))]
439  shape.pen.linewidth = child_linewidth
440 
441  # Recurse on this child
442  containers[child_path].set_styles(
443  selected_paths,
444  depth+1, max_depth,
445  items,
446  subgraph_shapes,
447  containers)
448  else:
449  if child_path in items:
450  for shape in items[child_path].shapes:
451  if not isinstance(shape,xdot.xdot.TextShape):
452  shape.pen.color = child_color
453  shape.pen.fillcolor = child_fillcolor
454  shape.pen.linewidth = child_linewidth
455  else:
456  #print child_path+" NOT IN "+str(items.keys())
457  pass
458 
459 class SmachViewerFrame(wx.Frame):
460  """
461  This class provides a GUI application for viewing SMACH plans.
462  """
463  def __init__(self):
464  wx.Frame.__init__(self, None, -1, "Smach Viewer", size=(720,480))
465 
466  # Create graph
467  self._containers = {}
468  self._top_containers = {}
469  self._update_cond = threading.Condition()
470  self._needs_refresh = True
471  self.dotstr = ''
472 
473  vbox = wx.BoxSizer(wx.VERTICAL)
474 
475 
476  # Create Splitter
477  self.content_splitter = wx.SplitterWindow(self, -1,style = wx.SP_LIVE_UPDATE)
478  self.content_splitter.SetMinimumPaneSize(24)
479  self.content_splitter.SetSashGravity(0.85)
480 
481 
482  # Create viewer pane
483  viewer = wx.Panel(self.content_splitter,-1)
484 
485  # Create smach viewer
486  nb = wx.Notebook(viewer,-1,style=wx.NB_TOP | wx.WANTS_CHARS)
487  viewer_box = wx.BoxSizer()
488  viewer_box.Add(nb,1,wx.EXPAND | wx.ALL, 4)
489  viewer.SetSizer(viewer_box)
490 
491  # Create graph view
492  graph_view = wx.Panel(nb,-1)
493  gv_vbox = wx.BoxSizer(wx.VERTICAL)
494  graph_view.SetSizer(gv_vbox)
495 
496  # Construct toolbar
497  toolbar = wx.ToolBar(graph_view, -1)
498 
499  toolbar.AddControl(wx.StaticText(toolbar,-1,"Path: "))
500 
501  # Path list
502  self.path_combo = wx.ComboBox(toolbar, -1, style=wx.CB_DROPDOWN)
503  self.path_combo .Bind(wx.EVT_COMBOBOX, self.set_path)
504  self.path_combo.Append('/')
505  self.path_combo.SetValue('/')
506  toolbar.AddControl(self.path_combo)
507 
508  # Depth spinner
509  self.depth_spinner = wx.SpinCtrl(toolbar, -1,
510  size=wx.Size(50,-1),
511  min=-1,
512  max=1337,
513  initial=-1)
514  self.depth_spinner.Bind(wx.EVT_SPINCTRL,self.set_depth)
515  self._max_depth = -1
516  toolbar.AddControl(wx.StaticText(toolbar,-1," Depth: "))
517  toolbar.AddControl(self.depth_spinner)
518 
519  # Label width spinner
520  self.width_spinner = wx.SpinCtrl(toolbar, -1,
521  size=wx.Size(50,-1),
522  min=1,
523  max=1337,
524  initial=40)
525  self.width_spinner.Bind(wx.EVT_SPINCTRL,self.set_label_width)
526  self._label_wrapper = textwrap.TextWrapper(40,break_long_words=True)
527  toolbar.AddControl(wx.StaticText(toolbar,-1," Label Width: "))
528  toolbar.AddControl(self.width_spinner)
529 
530  # Implicit transition display
531  toggle_all = wx.ToggleButton(toolbar,-1,'Show Implicit')
532  toggle_all.Bind(wx.EVT_TOGGLEBUTTON, self.toggle_all_transitions)
534 
535  toolbar.AddControl(wx.StaticText(toolbar,-1," "))
536  toolbar.AddControl(toggle_all)
537 
538  toggle_auto_focus = wx.ToggleButton(toolbar, -1, 'Auto Focus')
539  toggle_auto_focus.Bind(wx.EVT_TOGGLEBUTTON, self.toggle_auto_focus)
540  self._auto_focus = False
541 
542  toolbar.AddControl(wx.StaticText(toolbar, -1, " "))
543  toolbar.AddControl(toggle_auto_focus)
544 
545  toolbar.AddControl(wx.StaticText(toolbar,-1," "))
546  toolbar.AddLabelTool(wx.ID_HELP, 'Help',
547  wx.ArtProvider.GetBitmap(wx.ART_HELP,wx.ART_OTHER,(16,16)) )
548  toolbar.AddLabelTool(wx.ID_SAVE, 'Save',
549  wx.ArtProvider.GetBitmap(wx.ART_FILE_SAVE,wx.ART_OTHER,(16,16)) )
550  toolbar.Realize()
551 
552  self.Bind(wx.EVT_TOOL, self.ShowControlsDialog, id=wx.ID_HELP)
553  self.Bind(wx.EVT_TOOL, self.SaveDotGraph, id=wx.ID_SAVE)
554 
555  # Create dot graph widget
556  self.widget = xdot.wxxdot.WxDotWindow(graph_view, -1)
557 
558  gv_vbox.Add(toolbar, 0, wx.EXPAND)
559  gv_vbox.Add(self.widget, 1, wx.EXPAND)
560 
561  # Create tree view widget
562  self.tree = wx.TreeCtrl(nb,-1,style=wx.TR_HAS_BUTTONS)
563  nb.AddPage(graph_view,"Graph View")
564  nb.AddPage(self.tree,"Tree View")
565 
566 
567  # Create userdata widget
568  borders = wx.LEFT | wx.RIGHT | wx.TOP
569  border = 4
570  self.ud_win = wx.ScrolledWindow(self.content_splitter, -1)
571  self.ud_gs = wx.BoxSizer(wx.VERTICAL)
572 
573  self.ud_gs.Add(wx.StaticText(self.ud_win,-1,"Path:"),0, borders, border)
574 
575  self.path_input = wx.ComboBox(self.ud_win,-1,style=wx.CB_DROPDOWN)
576  self.path_input.Bind(wx.EVT_COMBOBOX,self.selection_changed)
577  self.ud_gs.Add(self.path_input,0,wx.EXPAND | borders, border)
578 
579 
580  self.ud_gs.Add(wx.StaticText(self.ud_win,-1,"Userdata:"),0, borders, border)
581 
582  self.ud_txt = wx.TextCtrl(self.ud_win,-1,style=wx.TE_MULTILINE | wx.TE_READONLY)
583  self.ud_gs.Add(self.ud_txt,1,wx.EXPAND | borders, border)
584 
585  # Add initial state button
586  self.is_button = wx.Button(self.ud_win,-1,"Set as Initial State")
587  self.is_button.Bind(wx.EVT_BUTTON, self.on_set_initial_state)
588  self.is_button.Disable()
589  self.ud_gs.Add(self.is_button,0,wx.EXPAND | wx.BOTTOM | borders, border)
590 
591  self.ud_win.SetSizer(self.ud_gs)
592 
593 
594  # Set content splitter
595  self.content_splitter.SplitVertically(viewer, self.ud_win, 512)
596 
597  # Add statusbar
598  self.statusbar = wx.StatusBar(self,-1)
599 
600  # Add elements to sizer
601  vbox.Add(self.content_splitter, 1, wx.EXPAND | wx.ALL)
602  vbox.Add(self.statusbar, 0, wx.EXPAND)
603 
604  self.SetSizer(vbox)
605  self.Center()
606 
607  # smach introspection client
608  self._client = smach_ros.IntrospectionClient()
609  self._containers= {}
610  self._selected_paths = []
611 
612  # Message subscribers
613  self._structure_subs = {}
614  self._status_subs = {}
615 
616  self.Bind(wx.EVT_IDLE,self.OnIdle)
617  self.Bind(wx.EVT_CLOSE,self.OnQuit)
618 
619  # Register mouse event callback
620  self.widget.register_select_callback(self.select_cb)
621  self._path = '/'
622  self._needs_zoom = True
623  self._structure_changed = True
624 
625  # Start a thread in the background to update the server list
626  self._keep_running = True
627  self._server_list_thread = threading.Thread(target=self._update_server_list)
628  self._server_list_thread.start()
629 
630  self._update_graph_thread = threading.Thread(target=self._update_graph)
631  self._update_graph_thread.start()
632  self._update_tree_thread = threading.Thread(target=self._update_tree)
633  self._update_tree_thread.start()
634 
635  def OnQuit(self,event):
636  """Quit Event: kill threads and wait for join."""
637  with self._update_cond:
638  self._keep_running = False
639  self._update_cond.notify_all()
640 
641  self._server_list_thread.join()
642  self._update_graph_thread.join()
643  self._update_tree_thread.join()
644 
645  event.Skip()
646 
647  def update_graph(self):
648  """Notify all that the graph needs to be updated."""
649  with self._update_cond:
650  self._update_cond.notify_all()
651 
652  def on_set_initial_state(self, event):
653  """Event: Change the initial state of the server."""
654  state_path = self._selected_paths[0]
655  parent_path = get_parent_path(state_path)
656  state = get_label(state_path)
657 
658  server_name = self._containers[parent_path]._server_name
659  self._client.set_initial_state(server_name,parent_path,[state],timeout = rospy.Duration(60.0))
660 
661  def set_path(self, event):
662  """Event: Change the viewable path and update the graph."""
663  self._path = self.path_combo.GetValue()
664  self._needs_zoom = True
665  self.update_graph()
666 
667  def _set_path(self, path):
668  self._path = path
669  self._needs_zoom = True
670  self.path_combo.SetValue(path)
671  self.update_graph()
672 
673  def set_depth(self, event):
674  """Event: Change the maximum depth and update the graph."""
675  self._max_depth = self.depth_spinner.GetValue()
676  self._needs_zoom = True
677  self.update_graph()
678 
679  def _set_max_depth(self, max_depth):
680  self._max_depth = max_depth
681  self.depth_spinner.SetValue(max_depth)
682  self._needs_zoom = True
683  self.update_graph()
684 
685  def set_label_width(self, event):
686  """Event: Change the label wrapper width and update the graph."""
687  self._label_wrapper.width = self.width_spinner.GetValue()
688  self._needs_zoom = True
689  self.update_graph()
690 
691  def toggle_all_transitions(self, event):
692  """Event: Change whether automatic transitions are hidden and update the graph."""
694  self._structure_changed = True
695  self.update_graph()
696 
697  def toggle_auto_focus(self, event):
698  """Event: Enable/Disable automatically focusing"""
699  self._auto_focus = not self._auto_focus
700  self._needs_zoom = self._auto_focus
701  self._structure_changed = True
702  if not self._auto_focus:
703  self._set_path('/')
704  self._max_depth(-1)
705  self.update_graph()
706 
707  def select_cb(self, item, event):
708  """Event: Click to select a graph node to display user data and update the graph."""
709 
710  # Only set string status
711  if not type(item.url) is str:
712  return
713 
714  self.statusbar.SetStatusText(item.url)
715  # Left button-up
716  if event.ButtonUp(wx.MOUSE_BTN_LEFT):
717  # Store this item's url as the selected path
718  self._selected_paths = [item.url]
719  # Update the selection dropdown
720  self.path_input.SetValue(item.url)
721  wx.PostEvent(
722  self.path_input.GetEventHandler(),
723  wx.CommandEvent(wx.wxEVT_COMMAND_COMBOBOX_SELECTED,self.path_input.GetId()))
724  self.update_graph()
725 
726  def selection_changed(self, event):
727  """Event: Selection dropdown changed."""
728  path_input_str = self.path_input.GetValue()
729 
730  # Check the path is non-zero length
731  if len(path_input_str) > 0:
732  # Split the path (state:outcome), and get the state path
733  path = path_input_str.split(':')[0]
734 
735  # Get the container corresponding to this path, since userdata is
736  # stored in the containers
737  if path not in self._containers:
738  parent_path = get_parent_path(path)
739  else:
740  parent_path = path
741 
742  if parent_path in self._containers:
743  # Enable the initial state button for the selection
744  self.is_button.Enable()
745 
746  # Get the container
747  container = self._containers[parent_path]
748 
749  # Store the scroll position and selection
750  pos = self.ud_txt.HitTestPos(wx.Point(0,0))
751  sel = self.ud_txt.GetSelection()
752 
753  # Generate the userdata string
754  ud_str = ''
755  for (k,v) in container._local_data._data.iteritems():
756  ud_str += str(k)+": "
757  vstr = str(v)
758  # Add a line break if this is a multiline value
759  if vstr.find('\n') != -1:
760  ud_str += '\n'
761  ud_str+=vstr+'\n\n'
762 
763  # Set the userdata string
764  self.ud_txt.SetValue(ud_str)
765 
766  # Restore the scroll position and selection
767  self.ud_txt.ShowPosition(pos[1])
768  if sel != (0,0):
769  self.ud_txt.SetSelection(sel[0],sel[1])
770  else:
771  # Disable the initial state button for this selection
772  self.is_button.Disable()
773 
774  def _structure_msg_update(self, msg, server_name):
775  """Update the structure of the SMACH plan (re-generate the dotcode)."""
776 
777  # Just return if we're shutting down
778  if not self._keep_running:
779  return
780 
781  # Get the node path
782  path = msg.path
783  pathsplit = path.split('/')
784  parent_path = '/'.join(pathsplit[0:-1])
785 
786  rospy.logdebug("RECEIVED: "+path)
787  rospy.logdebug("CONTAINERS: "+str(self._containers.keys()))
788 
789  # Initialize redraw flag
790  needs_redraw = False
791 
792  if path in self._containers:
793  rospy.logdebug("UPDATING: "+path)
794 
795  # Update the structure of this known container
796  needs_redraw = self._containers[path].update_structure(msg)
797  else:
798  rospy.logdebug("CONSTRUCTING: "+path)
799 
800  # Create a new container
801  container = ContainerNode(server_name, msg)
802  self._containers[path] = container
803 
804  # Store this as a top container if it has no parent
805  if parent_path == '':
806  self._top_containers[path] = container
807 
808  # Append paths to selector
809  self.path_combo.Append(path)
810  self.path_input.Append(path)
811 
812  # We need to redraw thhe graph if this container's parent is already known
813  if parent_path in self._containers:
814  needs_redraw = True
815 
816  # Update the graph if necessary
817  if needs_redraw:
818  with self._update_cond:
819  self._structure_changed = True
820  self._needs_zoom = True # TODO: Make it so you can disable this
821  self._update_cond.notify_all()
822 
823  def _status_msg_update(self, msg):
824  """Process status messages."""
825 
826  # Check if we're in the process of shutting down
827  if not self._keep_running:
828  return
829 
830  if self._auto_focus and len(msg.info) > 0:
831  self._set_path(msg.info)
832  self._set_max_depth(msg.info.count('/')-1)
833 
834  # Get the path to the updating conainer
835  path = msg.path
836  rospy.logdebug("STATUS MSG: "+path)
837 
838  # Check if this is a known container
839  if path in self._containers:
840  # Get the container and check if the status update requires regeneration
841  container = self._containers[path]
842  if container.update_status(msg):
843  with self._update_cond:
844  self._update_cond.notify_all()
845 
846  # TODO: Is this necessary?
847  path_input_str = self.path_input.GetValue()
848  if path_input_str == path or get_parent_path(path_input_str) == path:
849  wx.PostEvent(
850  self.path_input.GetEventHandler(),
851  wx.CommandEvent(wx.wxEVT_COMMAND_COMBOBOX_SELECTED,self.path_input.GetId()))
852 
853  def _update_graph(self):
854  """This thread continuously updates the graph when it changes.
855 
856  The graph gets updated in one of two ways:
857 
858  1: The structure of the SMACH plans has changed, or the display
859  settings have been changed. In this case, the dotcode needs to be
860  regenerated.
861 
862  2: The status of the SMACH plans has changed. In this case, we only
863  need to change the styles of the graph.
864  """
865  while self._keep_running and not rospy.is_shutdown():
866  with self._update_cond:
867  # Wait for the update condition to be triggered
868  self._update_cond.wait()
869 
870  # Get the containers to update
871  containers_to_update = {}
872  if self._path in self._containers:
873  # Some non-root path
874  containers_to_update = {self._path:self._containers[self._path]}
875  elif self._path == '/':
876  # Root path
877  containers_to_update = self._top_containers
878 
879  # Check if we need to re-generate the dotcode (if the structure changed)
880  # TODO: needs_zoom is a misnomer
881  if self._structure_changed or self._needs_zoom:
882  dotstr = "digraph {\n\t"
883  dotstr += ';'.join([
884  "compound=true",
885  "outputmode=nodesfirst",
886  "labeljust=l",
887  "nodesep=0.5",
888  "minlen=2",
889  "mclimit=5",
890  "clusterrank=local",
891  "ranksep=0.75",
892  # "remincross=true",
893  # "rank=sink",
894  "ordering=\"\"",
895  ])
896  dotstr += ";\n"
897 
898  # Generate the rest of the graph
899  # TODO: Only re-generate dotcode for containers that have changed
900  for path,tc in containers_to_update.iteritems():
901  dotstr += tc.get_dotcode(
902  self._selected_paths,[],
903  0,self._max_depth,
904  self._containers,
906  self._label_wrapper)
907  else:
908  dotstr += '"__empty__" [label="Path not available.", shape="plaintext"]'
909 
910  dotstr += '\n}\n'
911  self.dotstr = dotstr
912  # Set the dotcode to the new dotcode, reset the flags
913  self.set_dotcode(dotstr,zoom=False)
914  self._structure_changed = False
915 
916  # Update the styles for the graph if there are any updates
917  for path,tc in containers_to_update.iteritems():
918  tc.set_styles(
919  self._selected_paths,
920  0,self._max_depth,
921  self.widget.items_by_url,
922  self.widget.subgraph_shapes,
923  self._containers)
924 
925  # Redraw
926  self.widget.Refresh()
927 
928  def set_dotcode(self, dotcode, zoom=True):
929  """Set the xdot view's dotcode and refresh the display."""
930  # Set the new dotcode
931  if self.widget.set_dotcode(dotcode, None):
932  self.SetTitle('Smach Viewer')
933  # Re-zoom if necessary
934  if zoom or self._needs_zoom:
935  self.widget.zoom_to_fit()
936  self._needs_zoom = False
937  # Set the refresh flag
938  self._needs_refresh = True
939  wx.PostEvent(self.GetEventHandler(), wx.IdleEvent())
940 
941  def _update_tree(self):
942  """Update the tree view."""
943  while self._keep_running and not rospy.is_shutdown():
944  with self._update_cond:
945  self._update_cond.wait()
946  self.tree.DeleteAllItems()
947  self._tree_nodes = {}
948  for path,tc in self._top_containers.iteritems():
949  self.add_to_tree(path, None)
950 
951  def add_to_tree(self, path, parent):
952  """Add a path to the tree view."""
953  if parent is None:
954  container = self.tree.AddRoot(get_label(path))
955  else:
956  container = self.tree.AppendItem(parent,get_label(path))
957 
958  # Add children to tree
959  for label in self._containers[path]._children:
960  child_path = '/'.join([path,label])
961  if child_path in self._containers.keys():
962  self.add_to_tree(child_path, container)
963  else:
964  self.tree.AppendItem(container,label)
965 
966  def append_tree(self, container, parent = None):
967  """Append an item to the tree view."""
968  if not parent:
969  node = self.tree.AddRoot(container._label)
970  for child_label in container._children:
971  self.tree.AppendItem(node,child_label)
972 
973  def OnIdle(self, event):
974  """Event: On Idle, refresh the display if necessary, then un-set the flag."""
975  if self._needs_refresh:
976  self.Refresh()
977  # Re-populate path combo
978  self._needs_refresh = False
979 
981  """Update the list of known SMACH introspection servers."""
982  while self._keep_running:
983  # Update the server list
984  server_names = self._client.get_servers()
985  new_server_names = [sn for sn in server_names if sn not in self._status_subs]
986 
987  # Create subscribers for new servers
988  for server_name in new_server_names:
989  self._structure_subs[server_name] = rospy.Subscriber(
990  server_name+smach_ros.introspection.STRUCTURE_TOPIC,
991  SmachContainerStructure,
992  callback = self._structure_msg_update,
993  callback_args = server_name,
994  queue_size=50)
995 
996  self._status_subs[server_name] = rospy.Subscriber(
997  server_name+smach_ros.introspection.STATUS_TOPIC,
998  SmachContainerStatus,
999  callback = self._status_msg_update,
1000  queue_size=50)
1001 
1002  # This doesn't need to happen very often
1003  rospy.sleep(1.0)
1004 
1005 
1006  #self.server_combo.AppendItems([s for s in self._servers if s not in current_servers])
1007 
1008  # Grab the first server
1009  #current_value = self.server_combo.GetValue()
1010  #if current_value == '' and len(self._servers) > 0:
1011  # self.server_combo.SetStringSelection(self._servers[0])
1012  # self.set_server(self._servers[0])
1013 
1014  def ShowControlsDialog(self,event):
1015  dial = wx.MessageDialog(None,
1016  "Pan: Arrow Keys\nZoom: PageUp / PageDown\nZoom To Fit: F\nRefresh: R",
1017  'Keyboard Controls', wx.OK)
1018  dial.ShowModal()
1019 
1020  def SaveDotGraph(self,event):
1021  timestr = time.strftime("%Y%m%d-%H%M%S")
1022  directory = rospkg.get_ros_home()+'/dotfiles/'
1023  if not os.path.exists(directory):
1024  os.makedirs(directory)
1025  filename = directory+timestr+'.dot'
1026  print('Writing to file: %s' % filename)
1027  with open(filename, 'w') as f:
1028  f.write(self.dotstr)
1029 
1030  def OnExit(self, event):
1031  pass
1032 
1033  def set_filter(self, filter):
1034  self.widget.set_filter(filter)
1035 
1036 def main():
1037  from argparse import ArgumentParser
1038  p = ArgumentParser()
1039  p.add_argument('-f', '--auto-focus',
1040  action='store_true',
1041  help="Enable 'AutoFocus to subgraph' as default",
1042  dest='enable_auto_focus')
1043  args = p.parse_args()
1044  app = wx.App()
1045 
1046  frame = SmachViewerFrame()
1047  frame.set_filter('dot')
1048 
1049  frame.Show()
1050 
1051  if args.enable_auto_focus:
1052  frame.toggle_auto_focus(None)
1053 
1054  app.MainLoop()
1055 
1056 if __name__ == '__main__':
1057  rospy.init_node('smach_viewer',anonymous=False, disable_signals=True,log_level=rospy.INFO)
1058  sys.argv = rospy.myargv()
1059  main()
def append_tree(self, container, parent=None)
def select_cb(self, item, event)
def update_structure(self, msg)
def get_label(path)
def get_dotcode(self, selected_paths, closed_paths, depth, max_depth, containers, show_all, label_wrapper, attrs={})
def __init__(self, server_name, msg)
def attr_string(attrs)
Definition: smach_viewer.py:88
def hex2t(color_str)
def on_set_initial_state(self, event)
def update_status(self, msg)
def add_to_tree(self, path, parent)
def graph_attr_string(attrs)
Helper Functions.
Definition: smach_viewer.py:83
def import_non_local(name, custom_name=None)
this import system (or ros-released) xdot import xdot need to import currnt package, but not to load this file http://stackoverflow.com/questions/6031584/importing-from-builtin-library-when-module-with-same-name-exists
Definition: smach_viewer.py:62
def set_dotcode(self, dotcode, zoom=True)
def get_parent_path(path)
Definition: smach_viewer.py:93
def _status_msg_update(self, msg)
def toggle_all_transitions(self, event)
def set_styles(self, selected_paths, depth, max_depth, items, subgraph_shapes, containers)
def set_filter(self, filter)
def _set_max_depth(self, max_depth)
def ShowControlsDialog(self, event)
def _structure_msg_update(self, msg, server_name)
def set_label_width(self, event)
def toggle_auto_focus(self, event)
def selection_changed(self, event)


smach_viewer
Author(s): Jonathan Bohren
autogenerated on Fri Jun 7 2019 22:03:26