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
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060 import roslib
00061 roslib.load_manifest('cob_command_gui')
00062 from cob_relayboard.msg import EmergencyStopState
00063 from buttons import *
00064 import thread
00065 import pygtk
00066 pygtk.require('2.0')
00067 import gtk
00068 import roslib
00069 import os
00070 import pynotify
00071 import sys
00072 import signal
00073
00074 planning_enabled = False
00075 base_diff_enabled = False
00076 confirm_commands_enabled = True
00077
00078 initialized = False
00079
00080
00081 gtk.gdk.threads_init()
00082
00083
00084 def start(func, args):
00085 global planning_enabled
00086 global base_diff_enabled
00087 global confirm_commands_enabled
00088 execute_command = True
00089
00090 largs = list(args)
00091
00092 if confirm_commands_enabled and (largs[1] != 'stop'):
00093 confirm_dialog = gtk.MessageDialog(None, 0, gtk.MESSAGE_QUESTION, gtk.BUTTONS_YES_NO, "Execute Command?")
00094 if confirm_dialog.run() == gtk.RESPONSE_NO:
00095 execute_command = False
00096 confirm_dialog.destroy()
00097 if execute_command:
00098 if(largs[0] == "arm"):
00099 if(planning_enabled):
00100 largs.append("planned")
00101 if(largs[0] == "base"):
00102 if(base_diff_enabled):
00103 largs.append("diff")
00104
00105
00106 thread.start_new_thread(func,tuple(largs))
00107
00108 def startGTK(widget, data):
00109 data()
00110
00111
00112 class GtkGeneralPanel(gtk.Frame):
00113 def __init__(self,buttons):
00114 global initialized
00115 self.sss = simple_script_server()
00116 gtk.Frame.__init__(self)
00117 if not pynotify.init ("cob_command_gui"):
00118 sys.exit (1)
00119 self.em_stop = False
00120 self.set_label("general")
00121 self.set_shadow_type(gtk.SHADOW_IN)
00122 self.vbox = gtk.VBox(False, 0)
00123 self.add(self.vbox)
00124
00125
00126
00127
00128
00129
00130
00131 hbox=gtk.HBox(True, 0)
00132 self.status_image = gtk.Image()
00133
00134 hbox.pack_start(self.status_image, False, False, 0)
00135 self.status_label = gtk.Label("Status OK")
00136 hbox.pack_start(self.status_label, False, False, 0)
00137 self.vbox.pack_start(hbox, False, False, 5)
00138
00139 butstop = gtk.Button("Stop all")
00140 butstop.connect("clicked", lambda w: self.stop_all(buttons.stop_buttons))
00141 self.vbox.pack_start(butstop, False, False, 5)
00142
00143 butinit = gtk.Button("Init all")
00144 butinit.connect("clicked", lambda w: self.init_all(buttons.init_buttons))
00145 self.vbox.pack_start(butinit, False, False, 5)
00146
00147 butrec = gtk.Button("Recover all")
00148 butrec.connect("clicked", lambda w: self.recover_all(buttons.recover_buttons))
00149 self.vbox.pack_start(butrec, False, False, 5)
00150
00151 plan_check = gtk.CheckButton("Planning")
00152 plan_check.connect("toggled", self.planned_toggle)
00153 self.vbox.pack_start(plan_check, False, False, 5)
00154
00155 base_mode_check = gtk.CheckButton("Base Diff")
00156 base_mode_check.connect("toggled", self.base_mode_toggle)
00157 self.vbox.pack_start(base_mode_check, False, False, 5)
00158
00159 confirm_com_check = gtk.CheckButton("Confirm Commands")
00160 confirm_com_check.set_active(confirm_commands_enabled)
00161 confirm_com_check.connect("toggled", self.confirm_com_toggle)
00162 self.vbox.pack_start(confirm_com_check, False, False, 5)
00163
00164 but = gtk.Button(stock=gtk.STOCK_QUIT )
00165 but.connect("clicked", lambda w: gtk.main_quit())
00166 self.vbox.pack_start(but, False, False, 5)
00167 initialized = True
00168
00169 def stop_all(self,component_names):
00170 for component_name in component_names:
00171 self.sss.stop(component_name,blocking=False)
00172
00173 def init_all(self,component_names):
00174 for component_name in component_names:
00175 self.sss.init(component_name,False)
00176
00177 def recover_all(self,component_names):
00178 for component_name in component_names:
00179 self.sss.recover(component_name,False)
00180
00181 def setEMStop(self, em):
00182 if(em):
00183
00184 self.status_image.set_from_file(roslib.packages.get_pkg_dir("cob_command_gui") + "/common/files/icons/weather-storm.png")
00185 self.status_label.set_text("EM Stop !")
00186 if(self.em_stop == False):
00187 self.em_stop = True
00188 n = pynotify.Notification("Emergency Stop issued!", "", "dialog-warning")
00189 n.set_timeout(1)
00190 n.show()
00191 else:
00192
00193
00194 self.status_label.set_text("Status OK")
00195 if(self.em_stop == True):
00196 self.em_stop = False
00197 n = pynotify.Notification("Emergency Stop released!", "", "dialog-ok")
00198 n.set_timeout(1)
00199 n.show()
00200
00201 def planned_toggle(self, b):
00202 global planning_enabled
00203 if(planning_enabled):
00204 planning_enabled = False
00205 else:
00206 planning_enabled = True
00207
00208 def base_mode_toggle(self, b):
00209 global base_diff_enabled
00210 if(base_diff_enabled):
00211 base_diff_enabled = False
00212 else:
00213 base_diff_enabled = True
00214
00215 def confirm_com_toggle(self, b):
00216 global confirm_commands_enabled
00217 if(confirm_commands_enabled):
00218 confirm_commands_enabled = False
00219 else:
00220 confirm_commands_enabled = True
00221
00222
00223 class GtkPanel(gtk.Frame):
00224 def __init__(self, master=None, labeltext=""):
00225 gtk.Frame.__init__(self)
00226 self.set_label(labeltext)
00227 self.set_shadow_type(gtk.SHADOW_IN)
00228 self.vbox = gtk.VBox(False, 0)
00229 self.add(self.vbox)
00230
00231 def addButton(self, text, command):
00232 but = gtk.Button(text)
00233 but.connect("clicked", startGTK, command)
00234
00235 self.vbox.pack_start(but, False, False, 5)
00236
00237
00238 class Knoeppkes():
00239 def delete_event(self, widget, event, data=None):
00240 gtk.main_quit()
00241 return False
00242
00243 def emcb(self, msg):
00244 global initialized
00245 if(initialized):
00246 self.gpanel.setEMStop(msg.emergency_state)
00247
00248 def __init__(self):
00249
00250 rospy.init_node('cob_knoeppkes')
00251 rospy.Subscriber("/emergency_stop_state", EmergencyStopState, self.emcb)
00252
00253 self.window = gtk.Window(gtk.WINDOW_TOPLEVEL)
00254 self.window.connect("delete_event", self.delete_event)
00255 self.window.set_title("cob command gui")
00256 self.window.set_size_request(1000, 500)
00257 vbox = gtk.VBox(False, 1)
00258 self.hbox = gtk.HBox(True, 10)
00259 vbox.pack_start(self.hbox, True, True, 0)
00260 b = buttons()
00261 self.gpanel = GtkGeneralPanel(b)
00262 self.hbox.pack_start(self.gpanel,True, True, 3)
00263 panels = b.panels
00264 for pname, actions in panels:
00265 panel = GtkPanel(self, pname)
00266 for aname, func, args in actions:
00267 panel.addButton(text=aname, command=lambda f=func, a=args: start(f, a))
00268 self.hbox.pack_start(panel,True, True, 3)
00269
00270 self.status_bar = gtk.Statusbar()
00271 context_id = self.status_bar.get_context_id("Statusbar")
00272 string = "Connected to $ROS_MASTER_URI=" + os.environ.get("ROS_MASTER_URI")
00273 self.status_bar.push(context_id, string)
00274 vbox.pack_start(self.status_bar, False, False, 0)
00275 self.window.add(vbox)
00276 self.window.show_all()
00277 gtk.gdk.threads_init()
00278 gtk.main()
00279
00280 def signal_handler(signal, frame):
00281
00282 gtk.main_quit()
00283
00284 if __name__ == "__main__":
00285 signal.signal(signal.SIGINT, signal_handler)
00286 app = Knoeppkes()
00287