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
00009
00010 import sys
00011 import os.path as pt
00012 import time
00013 import signal
00014
00015
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
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):
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
00071 self.tabs = {}
00072 self.tool_dict = {}
00073
00074
00075
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
00083
00084
00085
00086 self.status_bar_msg = ''
00087
00088
00089
00090
00091
00092
00093
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
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
00146
00147
00148 def add_tools(self, tools_list):
00149
00150 self.button_group_tab = qtg.QButtonGroup()
00151
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
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
00165
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
00200 if self.graph_model.is_running():
00201 raise RuntimeError('Only state machine execution thread maybe be active at a time.')
00202
00203
00204
00205
00206
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
00210
00211 def _state_machine_status_cb(self, message):
00212 self.status_bar_msg = message
00213
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
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
00276
00277 container = self.ui.properties_container
00278
00279 cl = container.layout()
00280 cl.deleteLater()
00281 qtc.QCoreApplication.sendPostedEvents(cl, qtc.QEvent.DeferredDelete)
00282
00283 clayout = qtg.QVBoxLayout(container)
00284 clayout.setMargin(0)
00285
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
00295
00296
00297
00298
00299
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
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
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
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
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
00396
00397 old_node_name = tool_instance.get_current_node_name()
00398
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
00406 self.graph_model.replace_node(node, old_node_name)
00407
00408
00409
00410
00411
00412
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
00436
00437
00438
00439
00440
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
00447
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
00460
00461
00462
00463
00464
00465 def new_sm_cb(self):
00466
00467 if not self.check_current_document():
00468 return
00469
00470 self._set_model(gm.GraphModel())
00471 self.nothing_cb(None)
00472
00473
00474 def save_sm_cb(self):
00475
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
00484
00485 filename = str(qtg.QFileDialog.getSaveFileName(self, 'Save As', self.graph_model.document.get_filename()))
00486
00487
00488
00489
00490 if len(filename) == 0:
00491 return False
00492
00493 if pt.exists(filename):
00494
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
00511
00512
00513
00514
00515
00516
00517 def open_sm_cb(self):
00518
00519 if not self.check_current_document():
00520 return
00521
00522
00523 dialog = qtg.QFileDialog(self, 'Open State Machine', '~')
00524 dialog.setFileMode(qtg.QFileDialog.Directory)
00525 dialog.setViewMode(qtg.QFileDialog.List)
00526
00527
00528 if dialog.exec_():
00529 filenames = dialog.selectedFiles()
00530 filename = str(filenames[0])
00531
00532
00533 self.stop_sm_cb()
00534
00535
00536 self._set_model(gm.GraphModel.load(filename))
00537
00538
00539 self.fsm_stack = []
00540
00541
00542 self.nothing_cb(None)
00543 self._state_machine_status_cb(' ')
00544
00545
00546
00547
00548
00549
00550
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
00564
00565
00566
00567
00568
00569
00570
00571
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
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
00604 self.nothing_cb(None)
00605
00606
00607 def dclick_container_cb(self, fsm_stack_element):
00608
00609
00610 last_fsm_el = self.fsm_stack[-1]
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620 new_node = last_fsm_el.node.recreate(self.graph_model)
00621
00622
00623
00624
00625
00626 last_fsm_el.model.replace_node(new_node, last_fsm_el.node.get_name())
00627
00628
00629 self.fsm_stack = self.fsm_stack[:self.fsm_stack.index(fsm_stack_element)]
00630
00631
00632 self._set_model(fsm_stack_element.model, view=fsm_stack_element.view)
00633
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
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_())