cob_command_gui: cob_command_gui: knoeppkes.py Source File

00001 #!/usr/bin/python
00002 #################################################################
00003 ##\file
00004 #
00005 # \note
00006 #   Copyright (c) 2010 \n
00007 #   Fraunhofer Institute for Manufacturing Engineering
00008 #   and Automation (IPA) \n\n
00009 #
00010 #################################################################
00011 #
00012 # \note
00013 #   Project name: care-o-bot
00014 # \note
00015 #   ROS stack name: cob_apps
00016 # \note
00017 #   ROS package name: cob_command_gui
00018 #
00019 # \author
00020 #   Author: Florian Weisshardt, email:florian.weisshardt@ipa.fhg.de
00021 # \author
00022 #   Supervised by: Florian Weisshardt, email:florian.weisshardt@ipa.fhg.de
00023 #
00024 # \date Date of creation: Aug 2010
00025 #
00026 # \brief
00027 #   Implementation of ROS node for command_gui.
00028 #
00029 #################################################################
00030 #
00031 # Redistribution and use in source and binary forms, with or without
00032 # modification, are permitted provided that the following conditions are met:
00033 #
00034 #     - Redistributions of source code must retain the above copyright
00035 #       notice, this list of conditions and the following disclaimer. \n
00036 #     - Redistributions in binary form must reproduce the above copyright
00037 #       notice, this list of conditions and the following disclaimer in the
00038 #       documentation and/or other materials provided with the distribution. \n
00039 #     - Neither the name of the Fraunhofer Institute for Manufacturing
00040 #       Engineering and Automation (IPA) nor the names of its
00041 #       contributors may be used to endorse or promote products derived from
00042 #       this software without specific prior written permission. \n
00043 #
00044 # This program is free software: you can redistribute it and/or modify
00045 # it under the terms of the GNU Lesser General Public License LGPL as 
00046 # published by the Free Software Foundation, either version 3 of the 
00047 # License, or (at your option) any later version.
00048 # 
00049 # This program is distributed in the hope that it will be useful,
00050 # but WITHOUT ANY WARRANTY; without even the implied warranty of
00051 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00052 # GNU Lesser General Public License LGPL for more details.
00053 # 
00054 # You should have received a copy of the GNU Lesser General Public 
00055 # License LGPL along with this program. 
00056 # If not, see <http://www.gnu.org/licenses/>.
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 #Initializing the gtk's thread engine
00081 gtk.gdk.threads_init()
00082 
00083 ## Executes a button click in a new thread
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     #print "Args", tuple(largs)
00105     #print "func ", func
00106     thread.start_new_thread(func,tuple(largs))
00107 
00108 def startGTK(widget, data):
00109   data()
00110 
00111 ## Class for general gtk panel implementation
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     #hbox=gtk.HBox(True, 0)
00125     #image = gtk.Image()
00126     #image.set_from_file(roslib.packages.get_pkg_dir("cob_command_gui") + "/common/files/icons/batti-040.png")
00127     #hbox.pack_start(image, False, False, 0)
00128     #label = gtk.Label("40 %")
00129     #hbox.pack_start(label, False, False, 0)
00130     #self.vbox.pack_start(hbox, False, False, 5)    
00131     hbox=gtk.HBox(True, 0)
00132     self.status_image = gtk.Image()
00133     #self.status_image.set_from_file(roslib.packages.get_pkg_dir("cob_command_gui") + "/common/files/icons/weather-clear.png")
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       #print "Emergency Stop Active"
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       #print "Status OK"
00193       #self.status_image.set_from_file(roslib.packages.get_pkg_dir("cob_command_gui") + "/common/files/icons/weather-clear.png")
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 ## Class for gtk panel implementation
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     #but.set_size_request(120,-1)
00235     self.vbox.pack_start(but, False, False, 5)
00236         
00237 ## Implementation of knoeppkes command gui
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     # init ros node
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 #  print 'You pressed Ctrl+C!'
00282   gtk.main_quit()
00283 
00284 if __name__ == "__main__":
00285         signal.signal(signal.SIGINT, signal_handler)
00286         app = Knoeppkes()
00287