estop.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 #  E-stop GUI for ART vehicle
00004 #
00005 #   Copyright (C) 2010 Austin Robot Technology
00006 #   License: Modified BSD Software License Agreement
00007 #
00008 # $Id: estop.py 1027 2011-03-06 16:19:07Z jack.oquin $
00009 
00010 import sys
00011 import os
00012 import threading
00013 import wx
00014 
00015 PKG_NAME = 'art_nav'
00016 import roslib;
00017 roslib.load_manifest(PKG_NAME)
00018 import rospy
00019 
00020 from art_msgs.msg import Behavior
00021 from art_msgs.msg import EstopState
00022 from art_msgs.msg import NavigatorCommand
00023 from art_msgs.msg import NavigatorState
00024 
00025 last_state_ = EstopState()
00026 new_state_ = EstopState()
00027 new_behavior_ = Behavior.NONE
00028 
00029 cmd_ = rospy.Publisher('navigator/cmd', NavigatorCommand)
00030 rospy.init_node('estop')
00031 
00032 # set path name for resolving icons
00033 icons_path = os.path.join(roslib.packages.get_pkg_dir(PKG_NAME),
00034                           os.path.join("icons", "estop"))
00035 
00036 def check_state(state_msg):
00037     "check navigator state, request change if not desired state"
00038     global last_state_, new_state_, new_behavior_
00039     last_state_ = state_msg.estop.state
00040     rospy.logdebug('E-stop state: ' + str(last_state_))
00041     if last_state_ != new_state_ and new_behavior_ != Behavior.NONE:
00042         # send navigator command msg requesting new behavior
00043         cmd_msg = NavigatorCommand()
00044         cmd_msg.header.frame_id = '/map'
00045         cmd_msg.order.behavior.value = new_behavior_
00046         cmd_.publish(cmd_msg)
00047 
00048 def find_icon(dir, basename, extlist=['.svg', '.png']):
00049     """Find icon file with basename in dir.
00050 
00051     extlist: a list of possible extensions
00052     returns: full path name if file found, None otherwise
00053     """
00054     for ext in extlist:
00055         pathname = os.path.join(dir, basename + ext)
00056         if os.path.exists(pathname):
00057             return pathname
00058     return None
00059 
00060     # remember last state received
00061     last_state_ = state_msg
00062 
00063 def pkg_icon(name):
00064     """Resolve package-relative icon name to path name.
00065     """
00066     pname = find_icon(icons_path, name)
00067     if pname == None:
00068         raise NameError, 'icon file not found'
00069     return pname
00070 
00071 class MainWindow(wx.Frame):
00072     "Create main robot E-stop control window."
00073 
00074     def __init__(self, parent, title):
00075 
00076         wx.Frame.__init__(self, parent, title=title, size=wx.Size(600,200))
00077         self.statusbar = self.CreateStatusBar()
00078 
00079         # Setting up the menu.
00080         filemenu= wx.Menu()
00081         menuExit = filemenu.Append(wx.ID_EXIT,"E&xit"," Terminate the program")
00082         #filemenu.AppendSeparator()
00083 
00084         # Creating the menubar.
00085         menuBar = wx.MenuBar()
00086         menuBar.Append(filemenu,"&File")
00087         self.SetMenuBar(menuBar)
00088 
00089         toolbar = self.CreateToolBar()
00090         #toolbar.SetToolBitmapSize((48, 48))
00091         toolbar.AddLabelTool(wx.ID_EXIT, '', wx.Bitmap(pkg_icon('exit')))
00092         toolbar.AddLabelTool(wx.ID_FORWARD, '',
00093                              wx.Bitmap(pkg_icon('player_play')))
00094         toolbar.AddLabelTool(wx.ID_CANCEL, '',
00095                              wx.Bitmap(pkg_icon('player_pause')))
00096         toolbar.AddLabelTool(wx.ID_STOP, '',
00097                              wx.Bitmap(pkg_icon('player_stop')))
00098         toolbar.Realize()
00099 
00100         self.Bind(wx.EVT_TOOL, self.halt, id=wx.ID_EXIT)
00101         self.Bind(wx.EVT_MENU, self.halt, menuExit)
00102 
00103         self.Bind(wx.EVT_TOOL, self.run, id=wx.ID_FORWARD)
00104         self.Bind(wx.EVT_TOOL, self.pause, id=wx.ID_STOP)
00105         self.Bind(wx.EVT_TOOL, self.suspend, id=wx.ID_CANCEL)
00106 
00107         panel = wx.Panel(self, -1)
00108         panel.Bind(wx.EVT_KEY_DOWN, self.OnKeyDown)
00109         panel.SetFocus()
00110 
00111         self.Show(True)
00112 
00113     def halt(self, e):
00114         # send done command
00115         rospy.signal_shutdown('GUI exit')
00116         self.Close(True)                # Close the frame.
00117 
00118     def OnKeyDown(self, event):
00119         keycode = event.GetKeyCode()
00120         if keycode == wx.WXK_UP:
00121             self.run(event)
00122         elif keycode == wx.WXK_DOWN:
00123             self.pause(event)
00124         elif keycode == wx.WXK_PAUSE:
00125             self.suspend(event)
00126         else:
00127             event.Skip()
00128 
00129     def pause(self, e):
00130         "request immediate stop"
00131         global new_behavior_, new_state_
00132         new_state_ = EstopState.Pause
00133         new_behavior_ = Behavior.Pause
00134         self.statusbar.SetStatusText('Stopping')
00135         pass
00136 
00137     def run(self, e):
00138         "request autonomous run"
00139         global new_behavior_, new_state_
00140         new_state_ = EstopState.Run
00141         new_behavior_ = Behavior.Run
00142         self.statusbar.SetStatusText('Running')
00143 
00144     def suspend(self, e):
00145         "request suspension of autonomous operation"
00146         global new_behavior_, new_state_
00147         new_state_ = EstopState.Suspend
00148         new_behavior_ = Behavior.Suspend
00149         self.statusbar.SetStatusText('Suspending')
00150 
00151 
00152 class wxThread(threading.Thread):
00153 
00154     def __init__(self):
00155         self.topic = rospy.Subscriber('navigator/state', NavigatorState, check_state)
00156         threading.Thread.__init__(self)
00157 
00158     def run(self):
00159         # run the GUI
00160         app = wx.App(False)
00161         frame = MainWindow(None, 'ART robot E-stop control')
00162         exit_status = app.MainLoop()
00163         sys.exit(exit_status)
00164 
00165 if __name__ == '__main__':
00166 
00167     # run the program
00168     # needs two threads: GUI main loop and ROS event loop
00169     try:
00170         wxThread().start()
00171         rospy.spin()
00172     except rospy.ROSInterruptException: pass


art_nav
Author(s): Austin Robot Technology, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:08:43