rcommander.py
Go to the documentation of this file.
00001 import roslib; roslib.load_manifest('rcommander')
00002 
00003 import PyQt4.QtGui as qtg
00004 import PyQt4.QtCore as qtc
00005 from rcommander_auto import Ui_RCommanderWindow
00006 
00007 import rospy
00008 #import tf
00009 
00010 import sys
00011 import os.path as pt
00012 import time
00013 import signal
00014 
00015 #NodeBox 
00016 import graph
00017 
00018 import graph_view as gv
00019 import nodebox_gui as nbg
00020 import graph_model as gm
00021 import outcome_tool as ot
00022 import library_tool as lb
00023 import trigger_tool as tt
00024 #import pr2_utils as pu
00025 
00026 def split(num, factor):
00027     num1 = int(round(num * factor))
00028     num2 = num - num1
00029     return [num1, num2]
00030 
00031 class FSMStackElement:
00032 
00033     def __init__(self, model, view, node):
00034         self.model = model
00035         self.view = view
00036         self.graph_node = None
00037         self.node = node
00038 
00039 class RCommander(qtg.QMainWindow, nbg.NodeBoxGUI):
00040 
00041     def __init__(self, app): #, robot, tf_listener=None):
00042         qtg.QMainWindow.__init__(self)
00043         self.app = app
00044         self.ui = Ui_RCommanderWindow()
00045         self.ui.setupUi(self)
00046         nbg.NodeBoxGUI.__init__(self, self.ui.graphicsSuperView)
00047 
00048         self.connect(self.ui.run_button,         qtc.SIGNAL('clicked()'), self.run_cb)
00049         self.connect(self.ui.add_button,         qtc.SIGNAL('clicked()'), self.add_cb)
00050         self.connect(self.ui.reset_button,       qtc.SIGNAL('clicked()'), self.reset_cb)
00051         self.connect(self.ui.save_button,        qtc.SIGNAL('clicked()'), self.save_cb)
00052         self.connect(self.ui.start_state_button, qtc.SIGNAL('clicked()'), self.start_state_cb)
00053         self.connect(self.ui.add_to_library_button, qtc.SIGNAL('clicked()'), self.add_to_library_cb)
00054 
00055         self.connect(self.ui.delete_button, qtc.SIGNAL('clicked()'), self.delete_cb)
00056         self.connect(self.ui.action_Run, qtc.SIGNAL('triggered(bool)'), self.run_sm_cb)
00057         self.connect(self.ui.action_stop, qtc.SIGNAL('triggered(bool)'), self.stop_sm_cb)
00058         self.connect(self.ui.actionNew, qtc.SIGNAL('triggered(bool)'), self.new_sm_cb)
00059         self.connect(self.ui.action_save, qtc.SIGNAL('triggered(bool)'), self.save_sm_cb)
00060         self.connect(self.ui.action_save_as, qtc.SIGNAL('triggered(bool)'), self.save_as_sm_cb)
00061         self.connect(self.ui.action_open, qtc.SIGNAL('triggered(bool)'), self.open_sm_cb)
00062         self.connect(self.ui.action_quit, qtc.SIGNAL('triggered(bool)'), self.quit_cb)
00063         self.ui.splitter.setSizes(split(self.width(), .83))
00064 
00065         self.empty_container(self.ui.properties_tab)
00066         self.empty_container(self.ui.connections_tab)
00067         self.add_mode()
00068         self.disable_buttons()
00069 
00070         #create instance variables
00071         self.tabs = {}
00072         self.tool_dict = {}
00073 
00074         #name of currently selected tool that operates on graph
00075         #self.graph_model = None
00076         self.selected_tool = None
00077         self.selected_graph_tool = None 
00078         self.selected_node = None
00079         self.selected_edge = None
00080         self.fsm_stack = []
00081 
00082         #Setup animation timer
00083         #self.status_bar_timer = QTimer()
00084         #self.connect(self.status_bar_timer, SIGNAL('timeout()'), self.status_bar_check)
00085         #self.status_bar_timer.start(100)
00086         self.status_bar_msg = ''
00087         
00088         #Connect to ROS & PR2 
00089         #if tf_listener == None:
00090         #    tf_listener = tf.TransformListener()
00091         #self.tf_listener = tf_listener
00092         #self.robot = robot
00093         #self.pr2 = pu.PR2(self.tf_listener)
00094 
00095     def set_robot(self, robot, tf_listener):
00096         self.robot = robot
00097         self.tf_listener = tf_listener
00098 
00099     def status_bar_check(self):
00100         if self.graph_model.sm_thread.has_key('run_sm'):
00101             sm_thread = self.graph_model.sm_thread['run_sm']
00102 
00103             if sm_thread.exception != None:
00104                 m = sm_thread.exception.message
00105                 self.statusBar().showMessage('%s: %s' % (sm_thread.exception.__class__, m), 15000)
00106                 self.graph_model.sm_thread.pop('run_sm')
00107                 self.graph_model.sm_thread.pop('preempted')
00108                 return
00109 
00110             if sm_thread.outcome != None:
00111                 self.statusBar().showMessage('Finished with outcome: %s' % sm_thread.outcome, 15000)
00112                 self.graph_model.sm_thread.pop('run_sm')
00113                 self.graph_model.sm_thread.pop('preempted')
00114                 return
00115 
00116             if not sm_thread.isAlive():
00117                 self.statusBar().showMessage('Error: SM thread unexpectedly died.', 15000)
00118                 self.graph_model.sm_thread.pop('run_sm')
00119                 self.graph_model.sm_thread.pop('preempted')
00120                 return
00121 
00122             if self.graph_model.sm_thread['preempted'] != None and (time.time() - self.graph_model.sm_thread['preempted'] > 5.):
00123                 rospy.loginfo('Thread took too long to terminate.  Escallating and using exception exit.')
00124                 self.graph_model.sm_thread['run_sm'].except_preempt()
00125                 rospy.loginfo('Thread terminated.')
00126                 self.graph_model.sm_thread.pop('run_sm')
00127                 self.graph_model.sm_thread.pop('preempted')
00128 
00129             rstring = 'Running...'
00130             if str(self.statusBar().currentMessage()) != rstring:
00131                 self.statusBar().showMessage(rstring, 1000)
00132 
00133     ####################################################################################################################
00134     # GUI logic
00135     ####################################################################################################################
00136     def _create_tab(self, tab_name):
00137         ntab = qtg.QWidget()
00138         ntab.setObjectName(tab_name)
00139         qtg.QHBoxLayout(ntab)
00140         self.ui.tools_box.addTab(ntab, tab_name)
00141         self.ui.tools_box.setTabText(self.ui.tools_box.indexOf(ntab), tab_name)
00142         self.tabs[tab_name] = ntab
00143 
00144     ##
00145     # Should only be called once during initialization
00146     #
00147     # @param list of [tab-name, tool-object] pairs
00148     def add_tools(self, tools_list):
00149         #add tools to the right tab, creating tabs if needed
00150         self.button_group_tab = qtg.QButtonGroup()
00151         #self.connect(self.button_group_tab, qtc.SIGNAL('clicked(int)'), self.button_group_clicked_cb)
00152 
00153         for tab_name, tool in tools_list:
00154             if not self.tabs.has_key(tab_name):
00155                 self._create_tab(tab_name)
00156             tab_widget = self.tabs[tab_name]
00157             self.button_group_tab.addButton(tool.create_button(tab_widget))
00158             #self.tool_dict[tool.get_name()] = {'tool_obj': tool}
00159             self.tool_dict[tool.get_smach_class()] = {'tool_obj': tool}
00160 
00161         for tname in self.tabs.keys():
00162             self.tabs[tname].update()
00163 
00164         #self.tool_dict[outcome_tool.get_name()] = {'tool_obj': outcome_tool}
00165         #Outcome tool is a specialized built in tool
00166         self.button_group_tab.addButton(self.ui.add_outcome_button)
00167         outcome_tool = ot.OutcomeTool(self.ui.add_outcome_button, self)
00168         self.tool_dict[outcome_tool.get_smach_class()] = {'tool_obj': outcome_tool}
00169 
00170         self.button_group_tab.addButton(self.ui.library_button)
00171         library_tool = lb.LibraryTool(self.ui.library_button, self)
00172         self.tool_dict[library_tool.get_smach_class()] = {'tool_obj': library_tool}
00173 
00174     def empty_container(self, pbox): 
00175         layout = pbox.layout()
00176 
00177         for i in range(layout.count()):
00178             item = layout.itemAt(0)
00179             layout.removeItem(item)
00180         children = pbox.children()
00181 
00182         for c in children[1:]:
00183             try:
00184                 layout.removeWidget(c)
00185                 c.setParent(None)
00186             except TypeError, e:
00187                 pass
00188 
00189         layout.invalidate()
00190         pbox.update()
00191 
00192     def set_selected_tool(self, tool_name):
00193         self.selected_tool = tool_name
00194 
00195     def get_selected_tool(self):
00196         return self.selected_tool
00197 
00198     def run_state_machine(self, sm, graph_model):
00199         #if self.graph_model.sm_thread.has_key('run_sm'):
00200         if self.graph_model.is_running():
00201             raise RuntimeError('Only state machine execution thread maybe be active at a time.')
00202 
00203         #print 'run_state_machine called', self.graph_model.document.get_name()
00204         #print 'executing here'
00205         #print sm.execute()
00206         #print 'done executing'
00207         graph_model.register_status_cb(self._state_machine_status_cb)
00208         cur_sm_thread = graph_model.run(graph_model.document.get_name(), state_machine=sm)
00209         #self.statusBar().showMessage('Running state machine %s.' % graph_model.document.get_name())
00210 
00211     def _state_machine_status_cb(self, message):
00212         self.status_bar_msg = message
00213         #self.statusBar().showMessage(message)
00214 
00215     def check_current_document(self):
00216         if self.graph_model.document.modified:
00217             msg_box = qtg.QMessageBox()
00218             msg_box.setText('Current state machine has not been saved.')
00219             msg_box.setInformativeText('Do you want to save it first?')
00220             msg_box.setStandardButtons(qtg.QMessageBox.Yes | qtg.QMessageBox.No | qtg.QMessageBox.Cancel)
00221             msg_box.setDefaultButton(qtg.QMessageBox.Cancel)
00222             ret = msg_box.exec_()
00223 
00224             if ret == qtg.QMessageBox.Cancel:
00225                 return False
00226 
00227             elif ret == qtg.QMessageBox.Yes:
00228                 return self.save_sm_cb()
00229 
00230         return True
00231 
00232     def disable_buttons(self):
00233         self.ui.run_button.setDisabled(True)
00234         self.ui.reset_button.setDisabled(True)
00235         self.ui.add_button.setDisabled(True)
00236         self.ui.save_button.setDisabled(True)
00237 
00238     def enable_buttons(self):
00239         self.ui.run_button.setDisabled(False)
00240         self.ui.reset_button.setDisabled(False)
00241         self.ui.add_button.setDisabled(False)
00242         self.ui.save_button.setDisabled(False)
00243 
00244 
00245     def notify_deselected_button(self):
00246         selected_tool_class = self.get_selected_tool()
00247         if selected_tool_class != None:
00248             tool = self.tool_dict[selected_tool_class]['tool_obj']
00249             tool.deselect_tool()
00250 
00251     def deselect_tool_buttons(self):
00252         self.notify_deselected_button()
00253         self.button_group_tab.setExclusive(False)
00254         button = self.button_group_tab.checkedButton()
00255         #print button
00256         if button != None:
00257             button.setDown(False)
00258             button.setChecked(False)
00259         self.button_group_tab.setExclusive(True)
00260 
00261     def edit_mode(self):
00262         self.ui.add_button.hide()
00263         self.ui.save_button.show()
00264 
00265     def add_mode(self):
00266         self.ui.add_button.show()
00267         self.ui.save_button.hide()
00268 
00269     def empty_properties_box(self):
00270         self.empty_container(self.ui.properties_tab)
00271         self.empty_container(self.ui.connections_tab)
00272         self.empty_container(self.ui.properties_container)
00273 
00274         ##
00275         # Restore property tab's vbox
00276         ##
00277         container = self.ui.properties_container
00278         #Remove current layout and items
00279         cl = container.layout()
00280         cl.deleteLater()
00281         qtc.QCoreApplication.sendPostedEvents(cl, qtc.QEvent.DeferredDelete)
00282         #add in a new layout
00283         clayout = qtg.QVBoxLayout(container)
00284         clayout.setMargin(0)
00285         #add in a container widget
00286         self.ui.properties_tab = qtg.QWidget(container)
00287         pbox_layout = qtg.QFormLayout(self.ui.properties_tab)
00288         clayout.addWidget(self.ui.properties_tab)
00289         spacer = qtg.QSpacerItem(20, 40, qtg.QSizePolicy.Minimum, qtg.QSizePolicy.Expanding)
00290         clayout.addItem(spacer)
00291 
00292 
00293     ####################################################################################################################
00294     # Graph tools
00295     ####################################################################################################################
00296     #def _reconnect_states(self):
00297     #    for k in self.graph_model.states_dict:
00298     #        if hasattr(self.graph_model.get_state(k), 'set_robot'):
00299     #            self.graph_model.get_state(k).set_robot(self.robot)
00300 
00301     def connect_node(self, node):
00302         if hasattr(node, 'set_robot'): 
00303             node.set_robot(self.robot)
00304 
00305     def connection_changed(self, node_name, outcome_name, new_outcome):
00306         self.graph_model.connection_changed(node_name, outcome_name, new_outcome)
00307         #print 'connection_changed: State machine modified!'
00308         self.graph_model.document.modified = True
00309 
00310     def current_children_of(self, node_name):
00311         return self.graph_model.current_children_of(node_name)
00312 
00313     def connectable_nodes(self, node_name, outcome):
00314         return self.graph_model.connectable_nodes(node_name, outcome)
00315 
00316     def outputs_of_type(self, class_filter):
00317         return self.graph_model.outputs_of_type(class_filter)
00318 
00319     def set_selected_node(self, name):
00320         self.selected_node = name
00321 
00322     def set_selected_edge(self, n1, n2, label):
00323         if n1 == None:
00324             self.selected_edge = None
00325         else:
00326             self.selected_edge = self.graph_model.edge(n1, n2, label=label)
00327 
00328     ####################################################################################################################
00329     # All callbacks
00330     ####################################################################################################################
00331     def notify_activated(self):
00332         self.notify_deselected_button()
00333 
00334     def run_cb(self):
00335         if self.selected_tool == None:
00336             return
00337         try:
00338             tool_instance = self.tool_dict[self.selected_tool]['tool_obj']
00339             node = tool_instance.create_node(unique=False)
00340             singleton_sm, graph_model = self.graph_model.create_singleton_statemachine(node, self.robot)
00341             self.run_state_machine(singleton_sm, graph_model)
00342         except RuntimeError, e:
00343             qtg.QMessageBox.information(self, str(self.objectName()), 'RuntimeError: ' + e.message)
00344     
00345     def add_cb(self):
00346         if self.selected_tool == None:
00347             return
00348 
00349         #print 'selected tool ', self.selected_tool
00350         selected_tool = self.selected_tool
00351 
00352         tool_instance = self.tool_dict[selected_tool]['tool_obj']
00353         if hasattr(tool_instance, 'set_child_node'):
00354             if self.selected_node == None:
00355                 qtg.QMessageBox.information(self, str(self.objectName()), 'Need to have another node selected to create an instance of this node.')
00356                 return
00357             else:
00358                 state = self.graph_model.get_state(self.selected_node)
00359                 tool_instance.set_child_node(state)
00360 
00361         try:
00362             node = tool_instance.create_node()
00363             if node == None:
00364                 rospy.loginfo('For some reason node wasn\'t created')
00365                 return
00366             self.graph_model.add_node(node)
00367         except RuntimeError, e:
00368             qtg.QMessageBox.information(self, str(self.objectName()), 
00369                     'RuntimeError: ' + e.message)
00370             return 
00371 
00372         if self.selected_node == None:
00373             self.node_cb(self.graph_model.node(node.name))
00374         else:
00375             snode = self.graph_model.node(self.selected_node)
00376             if snode != None:
00377                 self.node_cb(snode)
00378             else:
00379                 self.nothing_cb(None)
00380                 #self.selected_node = None
00381 
00382         self.tool_dict[selected_tool]['tool_obj'].refresh_connections_box()
00383         self.graph_view.refresh()
00384         self.graph_model.document.modified = True
00385         tool_instance.clear_saved_state()
00386 
00387     def reset_cb(self):
00388         if self.selected_tool == None:
00389             return
00390         tool_instance = self.tool_dict[self.selected_tool]['tool_obj']
00391         tool_instance.reset()
00392         
00393     def save_cb(self):
00394         tool_instance = self.tool_dict[self.selected_tool]['tool_obj']
00395         #old_smach_node = self.graph_model.get_smach_state()
00396         #print 'save cb called!!!'
00397         old_node_name = tool_instance.get_current_node_name()
00398         # create a node with new settings
00399         try:
00400             node = tool_instance.create_node(unique=False)
00401         except RuntimeError, e:
00402             qtg.QMessageBox.information(self, str(self.objectName()), 
00403                     'RuntimeError: ' + e.message)
00404             return 
00405         # 'delete' old smach node
00406         self.graph_model.replace_node(node, old_node_name)
00407         #print 'TRANS!', smach_node.vels
00408         #self.graph_model.set_smach_state(old_smach_node.get_name(), smach_node)
00409 
00410         # connection changes are made instantly (so don't worry about them)
00411         # only saving of internal node parameters must be implemented by client tools
00412         #print 'save_cb: State machine modified!'
00413         self.graph_model.document.modified = True
00414 
00415     def start_state_cb(self):
00416         if self.selected_node != None:
00417             try:
00418                 self.graph_model.set_start_state(self.selected_node)
00419             except RuntimeError, e:
00420                 qtg.QMessageBox.information(self, str(self.objectName()), 'RuntimeError: ' + e.message)
00421 
00422     def add_to_library_cb(self):
00423         if self.selected_node != None:
00424             self.tool_dict['library']['tool_obj'].add_to_library(self.graph_model.get_state(self.selected_node))
00425 
00426     def delete_cb(self):
00427         if self.selected_node != None:
00428             if self.selected_node != 'start':
00429                 self.graph_model.delete_node(self.selected_node)
00430                 self.set_selected_node(None)
00431                 self.graph_view.refresh()
00432             else:
00433                 print 'Can\'t delete start node!'
00434 
00435         #TODO rethink deleting of edges
00436         #if self.selected_edge != None:
00437         #    se = self.selected_edge
00438         #    self.set_selected_edge(None, None)
00439         #    self.graph_model.delete_edge(se)
00440         #    self.graph_view.refresh()
00441         print 'delete_cb: State machine modified!'
00442         self.graph_model.document.modified = True
00443         self.nothing_cb(None)
00444 
00445     def run_sm_cb(self, checked):
00446         #TODO Disable all buttons.
00447         #TODO Reflect state of running graph.
00448         if self.graph_model.get_start_state() == None:
00449             qtg.QMessageBox.information(self, str(self.objectName()), \
00450                 'No start state set.  Select a state and click on \'Start State\' to set a new start state.')
00451         else:
00452             try:
00453                 self.run_state_machine(self.graph_model.create_state_machine(self.robot), self.graph_model)
00454             except RuntimeError, e:
00455                 qtg.QMessageBox.information(self, str(self.objectName()), 'RuntimeError: ' + e.message)
00456 
00457     def stop_sm_cb(self):
00458         self.graph_model.preempt()
00459         #if self.graph_model.is_running():
00460         #    self.graph_model.preempt()
00461         ##if self.graph_model.sm_thread.has_key('run_sm'):
00462         #    self.graph_model.sm_thread['run_sm'].preempt()
00463         #    self.graph_model.sm_thread['preempted'] = time.time()
00464 
00465     def new_sm_cb(self):
00466         #prompt user to save if document has been modifid
00467         if not self.check_current_document():
00468             return
00469 
00470         self._set_model(gm.GraphModel())
00471         self.nothing_cb(None)
00472         #self.document = FSMDocument.new_document()
00473 
00474     def save_sm_cb(self):
00475         #print 'has real filename?', self.document.has_real_filename()
00476         if self.graph_model.document.has_real_filename():
00477             self.graph_model.save(self.graph_model.document.get_filename())
00478             return True
00479         else:
00480             return self.save_as_sm_cb()
00481 
00482     def save_as_sm_cb(self):
00483         #popup file dialog
00484         #print 'save_as_sm_cb:before', rospy.is_shutdown()
00485         filename = str(qtg.QFileDialog.getSaveFileName(self, 'Save As', self.graph_model.document.get_filename()))
00486         #print 'save_as_sm_cb: after', rospy.is_shutdown()
00487         #self._fix_shutdown_flag()
00488 
00489         #user canceled
00490         if len(filename) == 0:
00491             return False
00492 
00493         if pt.exists(filename):
00494             #Ask if want to over write
00495             msg_box = qtg.QMessageBox()
00496             msg_box.setText('There is already a file with this name.')
00497             msg_box.setInformativeText('Do you want to overwrite it?')
00498             msg_box.setStandardButtons(qtg.QMessageBox.Yes | qtg.QMessageBox.No | qtg.QMessageBox.Cancel)
00499             msg_box.setDefaultButton(qtg.QMessageBox.Cancel)
00500             ret = msg_box.exec_()
00501             if ret == qtg.QMessageBox.No or ret == qtg.QMessageBox.Cancel:
00502                 return False
00503 
00504         self.graph_model.save(filename)
00505         self.graph_model.document.set_filename(filename)
00506         self.graph_model.document.real_filename = True
00507         self.graph_model.document.modified = False
00508         return True
00509 
00510     #def _fix_shutdown_flag(self):
00511     #    pass
00512     #    #TODO Figure out what this crap was about.
00513     #    #Some messed up bug with QFileDialog!!!
00514     #    #import rospy.core as rpc
00515     #    #rpc._shutdown_flag = False
00516 
00517     def open_sm_cb(self):
00518         #prompt user if current document has not been saved
00519         if not self.check_current_document():
00520             return
00521 
00522         #print 'open IS SHUTDOWN before', rospy.is_shutdown()
00523         dialog = qtg.QFileDialog(self, 'Open State Machine', '~')
00524         dialog.setFileMode(qtg.QFileDialog.Directory)
00525         dialog.setViewMode(qtg.QFileDialog.List)
00526 
00527         #self._fix_shutdown_flag()
00528         if dialog.exec_():
00529             filenames = dialog.selectedFiles()
00530             filename = str(filenames[0])
00531 
00532             #Stop things that are currently running
00533             self.stop_sm_cb()
00534 
00535             #Set this a the new model
00536             self._set_model(gm.GraphModel.load(filename))
00537 
00538             #TODO check that the top level state machine has been saved
00539             self.fsm_stack = []
00540 
00541             #Reset state of GUI
00542             self.nothing_cb(None)
00543             self._state_machine_status_cb(' ')
00544 
00545             #self.document = FSMDocument(filename, modified=False, real_filename=True)
00546 
00547         #print 'open IS SHUTDOWN after', rospy.is_shutdown()
00548 
00549     ####################################################################################################################
00550     # Graph Callbacks (called back from GraphModel)
00551     ####################################################################################################################
00552 
00553     def nothing_cb(self, pt):
00554         self.deselect_tool_buttons()
00555         self.set_selected_tool(None)
00556         self.set_selected_node(None)
00557         self.set_selected_edge(None, None, None)
00558         self.empty_properties_box()
00559         self.add_mode()
00560         self.disable_buttons()
00561 
00562     def node_cb(self, node):
00563         # When called back here, let the currently selected tool know that
00564         # it'll be *unselected*.
00565 
00566         # That tool would then save its state as a node (?) then
00567         # reload it when selected again.
00568         #   Would this interfere with node creation?
00569         #       only save nodes that have not been added to graph
00570         #       new names only if no saved nodes exist
00571         #       resetting *must* not create new names.
00572         self.deselect_tool_buttons()
00573 
00574         self.set_selected_node(node.id)
00575         self.set_selected_edge(None, None, None)
00576         state = self.graph_model.get_state(node.id)
00577 
00578         tool = self.tool_dict[state.__class__]['tool_obj']
00579         tool.button.setChecked(True)
00580         tool.activate_cb(state.get_name())
00581         self.set_selected_tool(state.__class__)
00582 
00583         self.edit_mode()
00584         self.enable_buttons()
00585         tool.node_selected(state)
00586 
00587         if state.is_runnable():
00588             self.ui.run_button.setDisabled(False)
00589         else:
00590             self.ui.run_button.setDisabled(True)
00591 
00592     def edge_cb(self, edge):
00593         self.set_selected_edge(edge.node1.id, edge.node2.id, edge.label)
00594         self.set_selected_node(None)
00595         self.disable_buttons()
00596 
00597     #Handler for double clicking on a node, descending a level
00598     def dclick_cb(self, node):
00599         snode = self.graph_model.get_state(node.id)
00600         if gm.is_container(snode):
00601             self.fsm_stack.append(FSMStackElement(self.graph_model, self.graph_view, snode))
00602             self._set_model(snode.get_child())
00603             #self._reconnect_states()
00604             self.nothing_cb(None)
00605 
00606     #Handler for double clicking on circle, ascending a level
00607     def dclick_container_cb(self, fsm_stack_element):
00608 
00609         #Store current model
00610         last_fsm_el = self.fsm_stack[-1]
00611         #last_fsm_el.node.set_child(self.graph_model)
00612 
00613         ######
00614         #recreate the old node with this new model as a child
00615         #each node need a function that lets you recreate it
00616         # what to call this? recreate? update?
00617         #       input: old node
00618         #       output: new node
00619         # Recreate replaces the old subtree with the current one being edited.
00620         new_node = last_fsm_el.node.recreate(self.graph_model)
00621         
00622         #replace old node in the graph, reserving links which exist
00623         # replace_node (fix it so that it works with new nodes of the same name)
00624         # restore_consistency
00625         #print 'new_smach_node', new_smach_node.get_name(), last_fsm_el.node.get_name()
00626         last_fsm_el.model.replace_node(new_node, last_fsm_el.node.get_name())
00627 
00628         #Shorten the stack to the element selected
00629         self.fsm_stack = self.fsm_stack[:self.fsm_stack.index(fsm_stack_element)]
00630 
00631         #Load the element we're given
00632         self._set_model(fsm_stack_element.model, view=fsm_stack_element.view)
00633         #self._reconnect_states()
00634         self.nothing_cb(None)
00635 
00636     def _set_model(self, model, view=None):
00637         self.graph_model = model
00638         if view == None:
00639             self.graph_view = gv.GraphView(self.context, self.graph_model)
00640             self.graph_view.setup()
00641         else:
00642             self.graph_view = view
00643 
00644         self.graph_model.gve.events.click = self.node_cb
00645         self.graph_model.gve.events.click_edge = self.edge_cb
00646         self.graph_model.gve.events.click_nothing = self.nothing_cb
00647         self.graph_model.gve.events.dclick = self.dclick_cb
00648         self.graph_view.fsm_dclick_cb = self.dclick_container_cb
00649 
00650     ####################################################################################################################
00651     # Drawing code
00652     ####################################################################################################################
00653     def setup(self):
00654         graph._ctx = self.context
00655         self.context.speed(30.)
00656         self.context.size(700, 700)
00657         self._set_model(gm.GraphModel())
00658 
00659     def draw(self, properties_dict):
00660         properties_dict['selected_edge'] = self.selected_edge
00661         properties_dict['selected_node'] = self.selected_node
00662         properties_dict['width'        ] = self.ui.graphicsSuperView.viewport().width()
00663         properties_dict['height'       ] = self.ui.graphicsSuperView.viewport().height()
00664         properties_dict['name'         ] = self.graph_model.document.get_name()
00665         properties_dict['fsm_stack'    ] = self.fsm_stack
00666         self.graph_view.draw(properties_dict)
00667 
00668         if str(self.statusBar().currentMessage()) != self.status_bar_msg:
00669             self.statusBar().showMessage(self.status_bar_msg)
00670 
00671     def quit_cb(self):
00672         self.stop_drawing()
00673         rospy.signal_shutdown('User closed window.')
00674         self.app.quit()
00675 
00676 def run_rcommander(plugin_namespace, robot=None, tf_listener=None):
00677     import plugins 
00678     import state_machine_tool as smt
00679     import sleep_tool as st
00680     import pointcloud_click_tool as ptl
00681     import freeze_frame_tool as frz
00682 
00683     app = qtg.QApplication(sys.argv)
00684     signal.signal(signal.SIGINT, signal.SIG_DFL)
00685     rc = RCommander(app)
00686     app.connect(app, qtc.SIGNAL('lastWindowClosed()'), app.quit)
00687     app.connect(rc.ui.action_quit, qtc.SIGNAL('clicked()'), app.quit)
00688     rc.set_robot(robot, tf_listener)
00689 
00690     default_tools = [['Origins', ptl.PointCloudClickTool(rc), 'default_frame'], 
00691                      ['Origins', frz.FreezeFrameTool(rc),     'default_frame'],
00692                      ['Misc',    smt.StateMachineTool(rc),    'default'], 
00693                      ['Misc',    st.SleepTool(rc),            'default'],
00694                      ['Misc',    tt.TriggerTool(rc),          'default']]
00695     tools_list = []
00696     for n,t,ns in default_tools:
00697         if ns in plugin_namespace:
00698             tools_list.append([n,t])
00699 
00700     plugin_clses = plugins.load_plugins(plugin_namespace)
00701     for tab_name, pcls in plugin_clses:
00702         tools_list.append([tab_name, pcls(rc)])
00703     rc.add_tools(tools_list)
00704 
00705     rc.show()
00706     sys.exit(app.exec_())


rcommander
Author(s): Hai Nguyen (haidai@gmail.com)
autogenerated on Thu Nov 28 2013 11:46:34