00001
00002
00003
00004
00005
00006
00007
00008
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
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
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
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
00080 filemenu= wx.Menu()
00081 menuExit = filemenu.Append(wx.ID_EXIT,"E&xit"," Terminate the program")
00082
00083
00084
00085 menuBar = wx.MenuBar()
00086 menuBar.Append(filemenu,"&File")
00087 self.SetMenuBar(menuBar)
00088
00089 toolbar = self.CreateToolBar()
00090
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
00115 rospy.signal_shutdown('GUI exit')
00116 self.Close(True)
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
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
00168
00169 try:
00170 wxThread().start()
00171 rospy.spin()
00172 except rospy.ROSInterruptException: pass