00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 from __future__ import with_statement
00040
00041 import roslib; roslib.load_manifest('application_manager')
00042 from PyQt4 import QtGui, QtCore
00043 import rospy
00044 import actionlib
00045 from actionlib_msgs.msg import GoalStatus
00046 import sys
00047 from application_msgs.msg import *
00048
00049
00050 class AppButton(QtGui.QPushButton):
00051
00052 def __init__(self, state, name, widget):
00053 QtGui.QPushButton.__init__(self, '', widget)
00054 self.name = name
00055 self.set_state(state)
00056 self.name_signal = QtCore.pyqtSignal()
00057 self.connect(self, QtCore.SIGNAL('clicked()'),
00058 self.button_click)
00059
00060 def set_state(self, state):
00061 self.state = state
00062 self.setText(self.name.split('/')[-1]+'\n'+self.state)
00063
00064
00065 def button_click(self):
00066 self.emit(QtCore.SIGNAL('clicked_name(str, str)'), self.name, self.state)
00067
00068
00069 class ManagerInterface():
00070 def __init__(self):
00071 self.list = actionlib.SimpleActionClient('application_manager/list_applications', ListApplicationsAction)
00072 self.run = actionlib.SimpleActionClient('application_manager/run_application', RunApplicationAction)
00073 self.stop = actionlib.SimpleActionClient('application_manager/stop_application', StopApplicationAction)
00074 self.kill = actionlib.SimpleActionClient('application_manager/kill_application', KillApplicationAction)
00075
00076 self.list.wait_for_server()
00077 self.run.wait_for_server()
00078 self.stop.wait_for_server()
00079 self.kill.wait_for_server()
00080
00081
00082 def change_state(self, application, state):
00083 if state == 'Stopped':
00084 self.run.send_goal(RunApplicationGoal(application, True))
00085 print 'Running %s'%application
00086 elif state == 'Running' or state == 'Starting':
00087 self.stop.send_goal_and_wait(StopApplicationGoal(application))
00088 print 'Stopping %s'%application
00089 elif state == 'Preempting':
00090 self.kill.send_goal_and_wait(KillApplicationGoal(application))
00091 print 'Killing %s'%application
00092
00093
00094 def get_state(self):
00095 self.list.send_goal_and_wait(ListApplicationsGoal())
00096 res = self.list.get_result()
00097 return zip(res.application_name, res.application_status)
00098
00099
00100
00101 class AppManagerGui(QtGui.QMainWindow):
00102 def __init__(self, parent=None):
00103 QtGui.QWidget.__init__(self, parent)
00104 self.manager = ManagerInterface()
00105
00106 self.setWindowTitle('Application Manager Interface')
00107 self.center()
00108
00109 self.buttons = {}
00110 self.update_button_state()
00111
00112
00113 self.timer = QtCore.QTimer(self)
00114 self.timer.connect(self.timer, QtCore.SIGNAL('timeout()'), self.update_button_state)
00115 self.timer.start(1000)
00116
00117
00118 def button_click(self, name, state):
00119 self.manager.change_state(name, state)
00120 self.update_button_state()
00121
00122
00123 def update_button_state(self):
00124 new_buttons = {}
00125 offset = 10
00126 for name, state in self.manager.get_state():
00127
00128 if not name in self.buttons:
00129 button = AppButton(state, name, self)
00130 button.connect(button, QtCore.SIGNAL('clicked_name(str, str)'), self.button_click)
00131 else:
00132 button = self.buttons[name]
00133 button.set_state(state)
00134 button.setGeometry(10, offset, 160, 70)
00135 new_buttons[name] = button
00136 offset = offset+80
00137 self.buttons = new_buttons
00138 self.resize(180,len(self.buttons)*80+10)
00139
00140 def center(self):
00141 screen = QtGui.QDesktopWidget().screenGeometry()
00142 size = self.geometry()
00143 self.move((screen.width()-size.width())/2, (screen.height()-size.height())/2)
00144
00145
00146 def main():
00147 app = QtGui.QApplication(sys.argv)
00148 app_manager_gui = AppManagerGui()
00149 app_manager_gui.show()
00150 sys.exit(app.exec_())
00151
00152
00153 if __name__ == '__main__':
00154 rospy.init_node('application_manager_do')
00155 main()
00156
00157
00158