class RCommander(qtg.QMainWindow, nbg.NodeBoxGUI):

    def __init__(self, app, width, height): #, robot, tf_listener=None):
        qtg.QMainWindow.__init__(self)
        self.size = [width, height]
        self.app = app
        self.ui = Ui_RCommanderWindow()
        self.ui.setupUi(self)
        self.resize(width, height)
        nbg.NodeBoxGUI.__init__(self, self.ui.graphicsSuperView)

        self.connect(self.ui.run_button,         qtc.SIGNAL('clicked()'), self.run_cb)
        self.connect(self.ui.add_button,         qtc.SIGNAL('clicked()'), self.add_cb)
        self.connect(self.ui.reset_button,       qtc.SIGNAL('clicked()'), self.reset_cb)
        self.connect(self.ui.save_button,        qtc.SIGNAL('clicked()'), self.save_cb)
        self.connect(self.ui.start_state_button, qtc.SIGNAL('clicked()'), self.start_state_cb)
        self.connect(self.ui.add_to_library_button, qtc.SIGNAL('clicked()'), self.add_to_library_cb)

        self.connect(self.ui.delete_button, qtc.SIGNAL('clicked()'), self.delete_cb)
        self.connect(self.ui.action_Run, qtc.SIGNAL('triggered(bool)'), self.run_sm_cb)
        self.connect(self.ui.action_stop, qtc.SIGNAL('triggered(bool)'), self.stop_sm_cb)
        self.connect(self.ui.actionNew, qtc.SIGNAL('triggered(bool)'), self.new_sm_cb)
        self.connect(self.ui.action_save, qtc.SIGNAL('triggered(bool)'), self.save_sm_cb)
        self.connect(self.ui.action_save_as, qtc.SIGNAL('triggered(bool)'), self.save_as_sm_cb)
        self.connect(self.ui.action_open, qtc.SIGNAL('triggered(bool)'), self.open_sm_cb)
        self.connect(self.ui.action_quit, qtc.SIGNAL('triggered(bool)'), self.quit_cb)
        self.ui.splitter.setSizes(split(self.width(), .83))

        self.empty_container(self.ui.properties_tab)
        self.empty_container(self.ui.connections_tab)
        self.add_mode()
        self.disable_buttons()

        #create instance variables
        self.tabs = {}
        self.tool_dict = {}

        #name of currently selected tool that operates on graph
        #self.graph_model = None
        self.selected_tool = None
        self.selected_graph_tool = None 
        self.selected_node = None
        self.selected_edge = None
        self.fsm_stack = []

        #Setup animation timer
        #self.status_bar_timer = QTimer()
        #self.connect(self.status_bar_timer, SIGNAL('timeout()'), self.status_bar_check)
        #self.status_bar_timer.start(100)
        self.status_bar_msg = ''
        self.status_bar_exception = None
        
        #Connect to ROS & PR2 
        #if tf_listener == None:
        #    tf_listener = tf.TransformListener()
        #self.tf_listener = tf_listener
        #self.robot = robot
        #self.pr2 = pu.PR2(self.tf_listener)

    def set_robot(self, robot, tf_listener):
        self.robot = robot
        self.tf_listener = tf_listener

    def status_bar_check(self):
        if self.graph_model.sm_thread.has_key('run_sm'):
            sm_thread = self.graph_model.sm_thread['run_sm']

            if sm_thread.exception != None:
                m = sm_thread.exception.message
                self.statusBar().showMessage('%s: %s' % (sm_thread.exception.__class__, m), 15000)
                self.graph_model.sm_thread.pop('run_sm')
                self.graph_model.sm_thread.pop('preempted')
                return

            if sm_thread.outcome != None:
                self.statusBar().showMessage('Finished with outcome: %s' % sm_thread.outcome, 15000)
                self.graph_model.sm_thread.pop('run_sm')
                self.graph_model.sm_thread.pop('preempted')
                return

            if not sm_thread.isAlive():
                self.statusBar().showMessage('Error: SM thread unexpectedly died.', 15000)
                self.graph_model.sm_thread.pop('run_sm')
                self.graph_model.sm_thread.pop('preempted')
                return

            if self.graph_model.sm_thread['preempted'] != None and (time.time() - self.graph_model.sm_thread['preempted'] > 5.):
                rospy.loginfo('Thread took too long to terminate.  Escallating and using exception exit.')
                self.graph_model.sm_thread['run_sm'].except_preempt()
                rospy.loginfo('Thread terminated.')
                self.graph_model.sm_thread.pop('run_sm')
                self.graph_model.sm_thread.pop('preempted')

            rstring = 'Running...'
            if str(self.statusBar().currentMessage()) != rstring:
                self.statusBar().showMessage(rstring, 1000)

    ####################################################################################################################
    # GUI logic
    ####################################################################################################################
    def _create_tab(self, tab_name):
        ntab = qtg.QWidget()
        ntab.setObjectName(tab_name)
        qtg.QHBoxLayout(ntab)
        self.ui.tools_box.addTab(ntab, tab_name)
        self.ui.tools_box.setTabText(self.ui.tools_box.indexOf(ntab), tab_name)
        self.tabs[tab_name] = ntab

    ##
    # Should only be called once during initialization
    #
    # @param list of [tab-name, tool-object] pairs
    def add_tools(self, tools_list):
        #add tools to the right tab, creating tabs if needed
        self.button_group_tab = qtg.QButtonGroup()
        #self.connect(self.button_group_tab, qtc.SIGNAL('clicked(int)'), self.button_group_clicked_cb)

        for tab_name, tool in tools_list:
            if not self.tabs.has_key(tab_name):
                self._create_tab(tab_name)
            tab_widget = self.tabs[tab_name]
            self.button_group_tab.addButton(tool.create_button(tab_widget))
            #self.tool_dict[tool.get_name()] = {'tool_obj': tool}
            self.tool_dict[tool.get_smach_class()] = {'tool_obj': tool}

        for tname in self.tabs.keys():
            self.tabs[tname].update()

        #self.tool_dict[outcome_tool.get_name()] = {'tool_obj': outcome_tool}
        #Outcome tool is a specialized built in tool
        self.button_group_tab.addButton(self.ui.add_outcome_button)
        outcome_tool = ot.OutcomeTool(self.ui.add_outcome_button, self)
        self.tool_dict[outcome_tool.get_smach_class()] = {'tool_obj': outcome_tool}

        self.button_group_tab.addButton(self.ui.library_button)
        library_tool = lb.LibraryTool(self.ui.library_button, self)
        self.tool_dict[library_tool.get_smach_class()] = {'tool_obj': library_tool}

    def empty_container(self, pbox): 
        layout = pbox.layout()

        for i in range(layout.count()):
            item = layout.itemAt(0)
            layout.removeItem(item)
        children = pbox.children()

        for c in children[1:]:
            try:
                layout.removeWidget(c)
                c.setParent(None)
            except TypeError, e:
                pass

        layout.invalidate()
        pbox.update()
Esempio n. 2
0
class RCommander(qtg.QMainWindow, nbg.NodeBoxGUI):
    def __init__(self, app):  #, robot, tf_listener=None):
        qtg.QMainWindow.__init__(self)
        self.app = app
        self.ui = Ui_RCommanderWindow()
        self.ui.setupUi(self)
        nbg.NodeBoxGUI.__init__(self, self.ui.graphicsSuperView)

        self.connect(self.ui.run_button, qtc.SIGNAL('clicked()'), self.run_cb)
        self.connect(self.ui.add_button, qtc.SIGNAL('clicked()'), self.add_cb)
        self.connect(self.ui.reset_button, qtc.SIGNAL('clicked()'),
                     self.reset_cb)
        self.connect(self.ui.save_button, qtc.SIGNAL('clicked()'),
                     self.save_cb)
        self.connect(self.ui.start_state_button, qtc.SIGNAL('clicked()'),
                     self.start_state_cb)
        self.connect(self.ui.add_to_library_button, qtc.SIGNAL('clicked()'),
                     self.add_to_library_cb)

        self.connect(self.ui.delete_button, qtc.SIGNAL('clicked()'),
                     self.delete_cb)
        self.connect(self.ui.action_Run, qtc.SIGNAL('triggered(bool)'),
                     self.run_sm_cb)
        self.connect(self.ui.action_stop, qtc.SIGNAL('triggered(bool)'),
                     self.stop_sm_cb)
        self.connect(self.ui.actionNew, qtc.SIGNAL('triggered(bool)'),
                     self.new_sm_cb)
        self.connect(self.ui.action_save, qtc.SIGNAL('triggered(bool)'),
                     self.save_sm_cb)
        self.connect(self.ui.action_save_as, qtc.SIGNAL('triggered(bool)'),
                     self.save_as_sm_cb)
        self.connect(self.ui.action_open, qtc.SIGNAL('triggered(bool)'),
                     self.open_sm_cb)
        self.connect(self.ui.action_quit, qtc.SIGNAL('triggered(bool)'),
                     self.quit_cb)
        self.ui.splitter.setSizes(split(self.width(), .83))

        self.empty_container(self.ui.properties_tab)
        self.empty_container(self.ui.connections_tab)
        self.add_mode()
        self.disable_buttons()

        #create instance variables
        self.tabs = {}
        self.tool_dict = {}

        #name of currently selected tool that operates on graph
        #self.graph_model = None
        self.selected_tool = None
        self.selected_graph_tool = None
        self.selected_node = None
        self.selected_edge = None
        self.fsm_stack = []

        #Setup animation timer
        #self.status_bar_timer = QTimer()
        #self.connect(self.status_bar_timer, SIGNAL('timeout()'), self.status_bar_check)
        #self.status_bar_timer.start(100)
        self.status_bar_msg = ''

        #Connect to ROS & PR2
        #if tf_listener == None:
        #    tf_listener = tf.TransformListener()
        #self.tf_listener = tf_listener
        #self.robot = robot
        #self.pr2 = pu.PR2(self.tf_listener)

    def set_robot(self, robot, tf_listener):
        self.robot = robot
        self.tf_listener = tf_listener

    def status_bar_check(self):
        if self.graph_model.sm_thread.has_key('run_sm'):
            sm_thread = self.graph_model.sm_thread['run_sm']

            if sm_thread.exception != None:
                m = sm_thread.exception.message
                self.statusBar().showMessage(
                    '%s: %s' % (sm_thread.exception.__class__, m), 15000)
                self.graph_model.sm_thread.pop('run_sm')
                self.graph_model.sm_thread.pop('preempted')
                return

            if sm_thread.outcome != None:
                self.statusBar().showMessage(
                    'Finished with outcome: %s' % sm_thread.outcome, 15000)
                self.graph_model.sm_thread.pop('run_sm')
                self.graph_model.sm_thread.pop('preempted')
                return

            if not sm_thread.isAlive():
                self.statusBar().showMessage(
                    'Error: SM thread unexpectedly died.', 15000)
                self.graph_model.sm_thread.pop('run_sm')
                self.graph_model.sm_thread.pop('preempted')
                return

            if self.graph_model.sm_thread['preempted'] != None and (
                    time.time() - self.graph_model.sm_thread['preempted'] >
                    5.):
                rospy.loginfo(
                    'Thread took too long to terminate.  Escallating and using exception exit.'
                )
                self.graph_model.sm_thread['run_sm'].except_preempt()
                rospy.loginfo('Thread terminated.')
                self.graph_model.sm_thread.pop('run_sm')
                self.graph_model.sm_thread.pop('preempted')

            rstring = 'Running...'
            if str(self.statusBar().currentMessage()) != rstring:
                self.statusBar().showMessage(rstring, 1000)

    ####################################################################################################################
    # GUI logic
    ####################################################################################################################
    def _create_tab(self, tab_name):
        ntab = qtg.QWidget()
        ntab.setObjectName(tab_name)
        qtg.QHBoxLayout(ntab)
        self.ui.tools_box.addTab(ntab, tab_name)
        self.ui.tools_box.setTabText(self.ui.tools_box.indexOf(ntab), tab_name)
        self.tabs[tab_name] = ntab

    ##
    # Should only be called once during initialization
    #
    # @param list of [tab-name, tool-object] pairs
    def add_tools(self, tools_list):
        #add tools to the right tab, creating tabs if needed
        self.button_group_tab = qtg.QButtonGroup()
        #self.connect(self.button_group_tab, qtc.SIGNAL('clicked(int)'), self.button_group_clicked_cb)

        for tab_name, tool in tools_list:
            if not self.tabs.has_key(tab_name):
                self._create_tab(tab_name)
            tab_widget = self.tabs[tab_name]
            self.button_group_tab.addButton(tool.create_button(tab_widget))
            #self.tool_dict[tool.get_name()] = {'tool_obj': tool}
            self.tool_dict[tool.get_smach_class()] = {'tool_obj': tool}

        for tname in self.tabs.keys():
            self.tabs[tname].update()

        #self.tool_dict[outcome_tool.get_name()] = {'tool_obj': outcome_tool}
        #Outcome tool is a specialized built in tool
        self.button_group_tab.addButton(self.ui.add_outcome_button)
        outcome_tool = ot.OutcomeTool(self.ui.add_outcome_button, self)
        self.tool_dict[outcome_tool.get_smach_class()] = {
            'tool_obj': outcome_tool
        }

        self.button_group_tab.addButton(self.ui.library_button)
        library_tool = lb.LibraryTool(self.ui.library_button, self)
        self.tool_dict[library_tool.get_smach_class()] = {
            'tool_obj': library_tool
        }

    def empty_container(self, pbox):
        layout = pbox.layout()

        for i in range(layout.count()):
            item = layout.itemAt(0)
            layout.removeItem(item)
        children = pbox.children()

        for c in children[1:]:
            try:
                layout.removeWidget(c)
                c.setParent(None)
            except TypeError, e:
                pass

        layout.invalidate()
        pbox.update()
Esempio n. 3
0
class RCommander(QMainWindow, nbg.NodeBoxGUI):

    def __init__(self):
        QMainWindow.__init__(self)
        self.ui = Ui_RCommanderWindow()
        self.ui.setupUi(self)
        nbg.NodeBoxGUI.__init__(self, self.ui.graphicsSuperView)

        self.connect(self.ui.run_button,         SIGNAL('clicked()'), self.run_cb)
        self.connect(self.ui.add_button,         SIGNAL('clicked()'), self.add_cb)
        self.connect(self.ui.reset_button,       SIGNAL('clicked()'), self.reset_cb)
        self.connect(self.ui.save_button,        SIGNAL('clicked()'), self.save_cb)
        self.connect(self.ui.start_state_button, SIGNAL('clicked()'), self.start_state_cb)

        self.connect(self.ui.delete_button, SIGNAL('clicked()'), self.delete_cb)
        self.connect(self.ui.action_Run, SIGNAL('triggered(bool)'), self.run_sm_cb)
        self.connect(self.ui.action_stop, SIGNAL('triggered(bool)'), self.stop_sm_cb)
        self.connect(self.ui.actionNew, SIGNAL('triggered(bool)'), self.new_sm_cb)
        self.connect(self.ui.action_save, SIGNAL('triggered(bool)'), self.save_sm_cb)
        self.connect(self.ui.action_save_as, SIGNAL('triggered(bool)'), self.save_as_sm_cb)
        self.connect(self.ui.action_open, SIGNAL('triggered(bool)'), self.open_sm_cb)
        self.ui.splitter.setSizes(split(self.width(), .83))

        self.empty_container(self.ui.properties_tab)
        self.empty_container(self.ui.connections_tab)
        self.add_mode()
        self.disable_buttons()

        #create instance variables
        self.tabs = {}
        self.tool_dict = {}

        #name of currently selected tool that operates on graph
        #self.graph_model = None
        self.selected_tool = None
        self.selected_graph_tool = None 
        self.selected_node = None
        self.selected_edge = None
        self.fsm_stack = []

        #Setup animation timer
        self.status_bar_timer = QTimer()
        self.connect(self.status_bar_timer, SIGNAL('timeout()'), self.status_bar_check)
        self.status_bar_timer.start(100)
        
        #Connect to ROS & PR2 
        rospy.init_node('rcommander', anonymous=True)
        self.tf_listener = tf.TransformListener()
        self.pr2 = pu.PR2(self.tf_listener)


    def status_bar_check(self):
        if self.graph_model.sm_thread.has_key('run_sm'):
            sm_thread = self.graph_model.sm_thread['run_sm']

            if sm_thread.exception != None:
                m = sm_thread.exception.message
                self.statusBar().showMessage('%s: %s' % (sm_thread.exception.__class__, m), 15000)
                self.graph_model.sm_thread.pop('run_sm')
                self.graph_model.sm_thread.pop('preempted')
                return

            if sm_thread.outcome != None:
                self.statusBar().showMessage('Finished with outcome: %s' % sm_thread.outcome, 15000)
                self.graph_model.sm_thread.pop('run_sm')
                self.graph_model.sm_thread.pop('preempted')
                return

            if not sm_thread.isAlive():
                self.statusBar().showMessage('Error: SM thread unexpectedly died.', 15000)
                self.graph_model.sm_thread.pop('run_sm')
                self.graph_model.sm_thread.pop('preempted')
                return

            if self.graph_model.sm_thread['preempted'] != None and (time.time() - self.graph_model.sm_thread['preempted'] > 5.):
                rospy.loginfo('Thread took too long to terminate.  Escallating and using exception exit.')
                self.graph_model.sm_thread['run_sm'].except_preempt()
                rospy.loginfo('Thread terminated.')
                self.graph_model.sm_thread.pop('run_sm')
                self.graph_model.sm_thread.pop('preempted')

            rstring = 'Running...'
            if str(self.statusBar().currentMessage()) != rstring:
                self.statusBar().showMessage(rstring, 1000)


    ####################################################################################################################
    # GUI logic
    ####################################################################################################################
    def _create_tab(self, tab_name):
        ntab = QWidget()
        ntab.setObjectName(tab_name)
        QHBoxLayout(ntab)
        self.ui.tools_box.addTab(ntab, tab_name)
        self.ui.tools_box.setTabText(self.ui.tools_box.indexOf(ntab), tab_name)
        self.tabs[tab_name] = ntab

    ##
    # Should only be called once during initialization
    #
    # @param list of [tab-name, tool-object] pairs
    def add_tools(self, tools_list):
        #add tools to the right tab, creating tabs if needed
        self.button_group_tab = QButtonGroup()
        for tab_name, tool in tools_list:
            if not self.tabs.has_key(tab_name):
                self._create_tab(tab_name)
            tab_widget = self.tabs[tab_name]
            self.button_group_tab.addButton(tool.create_button(tab_widget))
            #self.tool_dict[tool.get_name()] = {'tool_obj': tool}
            self.tool_dict[tool.get_smach_class()] = {'tool_obj': tool}

        for tname in self.tabs.keys():
            self.tabs[tname].update()

        #Outcome tool is a specialized built in tool
        self.button_group_tab.addButton(self.ui.add_outcome_button)
        outcome_tool = ot.OutcomeTool(self.ui.add_outcome_button, self)
        #self.tool_dict[outcome_tool.get_name()] = {'tool_obj': outcome_tool}
        self.tool_dict[outcome_tool.get_smach_class()] = {'tool_obj': outcome_tool}

    def empty_container(self, pbox): 
        #pbox = self.ui.properties_tab
        formlayout = pbox.layout()
        for i in range(formlayout.count()):
            item = formlayout.itemAt(0)
            formlayout.removeItem(item)
        children = pbox.children()
        for c in children[1:]:
            formlayout.removeWidget(c)
            c.setParent(None)
        formlayout.invalidate()
        pbox.update()

    def set_selected_tool(self, tool_name):
        self.selected_tool = tool_name

    def run_state_machine(self, sm):
        if self.graph_model.sm_thread.has_key('run_sm'):
            raise RuntimeError('Only state machine execution thread maybe be active at a time.')
        self.graph_model.run(self.graph_model.document.get_name(), state_machine=sm)

    def check_current_document(self):
        if self.graph_model.document.modified:
            msg_box = QMessageBox()
            msg_box.setText('Current state machine has not been saved.')
            msg_box.setInformativeText('Do you want to save it first?')
            msg_box.setStandardButtons(QMessageBox.Yes | QMessageBox.No | QMessageBox.Cancel)
            msg_box.setDefaultButton(QMessageBox.Cancel)
            ret = msg_box.exec_()

            if ret == QMessageBox.Cancel:
                return False

            elif ret == QMessageBox.Yes:
                return self.save_sm_cb()

        return True

    def disable_buttons(self):
        self.ui.run_button.setDisabled(True)
        self.ui.reset_button.setDisabled(True)
        self.ui.add_button.setDisabled(True)
        self.ui.save_button.setDisabled(True)

    def enable_buttons(self):
        self.ui.run_button.setDisabled(False)
        self.ui.reset_button.setDisabled(False)
        self.ui.add_button.setDisabled(False)
        self.ui.save_button.setDisabled(False)

    def deselect_tool_buttons(self):
        self.button_group_tab.setExclusive(False)
        button = self.button_group_tab.checkedButton()
        #print button
        if button != None:
            #print 'checked', button.isChecked(), button.isDown(), button.isCheckable(), button.text()
            button.setDown(False)
            button.setChecked(False)
            #print 'checked2', button.isChecked(), button.isDown()
        self.button_group_tab.setExclusive(True)

    def edit_mode(self):
        self.ui.add_button.hide()
        self.ui.save_button.show()

    def add_mode(self):
        self.ui.add_button.show()
        self.ui.save_button.hide()

    def empty_properties_box(self):
        self.empty_container(self.ui.properties_tab)
        self.empty_container(self.ui.connections_tab)

    ####################################################################################################################
    # Graph tools
    ####################################################################################################################

    def _reconnect_smach_states(self):
        for k in self.graph_model.smach_states:
            if hasattr(self.graph_model.smach_states[k], 'set_robot'):
                self.graph_model.smach_states[k].set_robot(self.pr2)

    def connection_changed(self, node_name, outcome_name, new_outcome):
        self.graph_model.connection_changed(node_name, outcome_name, new_outcome)
        self.graph_model.document.modified = True

    def current_children_of(self, node_name):
        return self.graph_model.current_children_of(node_name)

    def connectable_nodes(self, node_name, outcome):
        return self.graph_model.connectable_nodes(node_name, outcome)

    def global_nodes(self, class_filter):
        return self.graph_model.global_nodes(class_filter)

    def set_selected_node(self, name):
        self.selected_node = name

    def set_selected_edge(self, n1, n2, label):
        if n1 == None:
            self.selected_edge = None
        else:
            self.selected_edge = self.graph_model.edge(n1, n2, label=label)

    ####################################################################################################################
    # All callbacks
    ####################################################################################################################
    def run_cb(self):
        if self.selected_tool == None:
            return
        try:
            tool_instance = self.tool_dict[self.selected_tool]['tool_obj']
            node = tool_instance.create_node(unique=False)
            singleton_sm = self.graph_model.create_singleton_statemachine(node)
            self.run_state_machine(singleton_sm)
        except RuntimeError, e:
            QMessageBox.information(self, str(self.objectName()), 'RuntimeError: ' + e.message)