Statistics
| Branch: | Revision:

root / scout / scoutsim / BehaviorGUI.py @ 5755691e

History | View | Annotate | Download (6.89 KB)

1
#!/usr/bin/python
2

    
3
import wx
4
import wx.lib.intctrl 
5
import string
6
import subprocess
7
import signal
8
from collections import defaultdict
9
# ROS imports
10
import roslib; roslib.load_manifest("scoutsim")
11
import rospy
12
from scoutsim.srv import *
13

    
14
# Everyone likes some nice globals
15
scouts = defaultdict(str) # Associative array in the form [scout_name: current_behavior]
16
processes = defaultdict() # Associative array in the form [scout_name: process_handler]
17

    
18
#
19
# ROS functions
20
#
21
def AddToSimulator(name, x, y, theta):
22
        if name in scouts:
23
                return False
24
        scouts[name] = ""
25
        print "Calling ROS function to create scout "+name
26
        try:
27
                rospy.wait_for_service('/spawn');
28
                service = rospy.ServiceProxy('/spawn', Spawn);
29
                resp = service(x, y, theta, name)
30
                return True
31
        except rospy.ServiceException, e:
32
                return False
33

    
34
def GetBehaviors():
35
        return ["CW Circle", "CCW Circle", "Odometry", "Navigation Map", "Scheduler", "Warehouse", "Line Follow", "WL Test"]
36

    
37
def SetBehavior(name, behaviorLabel, behaviorButton=False, behavior="Pause"):
38
        if behavior == "Pause":
39
                if name not in processes:
40
                        return False
41
                if not behaviorButton.GetValue():
42
                        processes[name].send_signal(signal.SIGCONT)
43
                        print "Scout "+name+" was resumed."
44
                        behaviorButton.SetLabel("Pause")
45
                        behaviorLabel.SetLabel("  |  Current Behavior: "+scouts[name])
46
                else:
47
                        processes[name].send_signal(signal.SIGSTOP)
48
                        print "Scout "+name+" was paused."
49
                        behaviorButton.SetLabel("Resume")
50
                        behaviorLabel.SetLabel("  |  Current Behavior: Paused")
51
                return True
52
        if name in processes:
53
                processes[name].kill()
54
        processes[name] = subprocess.Popen("rosrun libscout libscout "+name+" "+str(GetBehaviors().index(behavior))+"", shell=True) 
55
        print "Scout "+name+" was set to behavior: "+behavior
56
        behaviorLabel.SetLabel("  |  Current Behavior: "+behavior)
57
        scouts[name] = behavior
58

    
59
def KillScout(name):
60
        try:
61
                rospy.wait_for_service('/kill');
62
                service = rospy.ServiceProxy('/kill', Kill);
63
                if name in processes:
64
                        processes[name].kill() # This is perhaps not working
65
                resp = service(name)
66
                del scouts[name]
67
                del processes[name]
68
                return True
69
        except rospy.ServiceException, e:
70
                return False
71

    
72
def PauseScout(name, behaviorLabel, behaviorButton):
73
        return SetBehavior(name, behaviorLabel, behaviorButton)
74
#
75
# End ROS functions
76
#
77

    
78

    
79
class GUI(wx.Frame):
80
        def __init__(self, parent, title):
81
                super(GUI, self).__init__(parent, title=title, style=wx.DEFAULT_FRAME_STYLE ^ wx.RESIZE_BORDER, size=(600, 600))
82
                self.InitUI()
83
                self.Show()     
84

    
85
        def InitUI(self):
86
                self.window = wx.ScrolledWindow(self, style=wx.VSCROLL)
87
                self.mainArea = wx.GridSizer(cols=1)
88
                sizer = wx.FlexGridSizer(rows=4, cols=8, hgap=5, vgap=5)
89

    
90
                # Labels
91
                blankText = wx.StaticText(self.window, label="")
92
                newScout = wx.StaticText(self.window, label="New Scout")
93
                newScoutName = wx.StaticText(self.window, label="Name:")
94
                startXTitle = wx.StaticText(self.window, label="X:")
95
                startYTitle = wx.StaticText(self.window, label="Y:")
96
                startThetaTitle = wx.StaticText(self.window, label=u'\u0398')
97

    
98
                # Inputs
99
                newScoutInput = wx.TextCtrl(self.window)
100
                startX = wx.lib.intctrl.IntCtrl(self.window, min=0)
101
                startY = wx.lib.intctrl.IntCtrl(self.window, min=0)
102
                startTheta = wx.lib.intctrl.IntCtrl(self.window, min=0, max=360)
103
                addButton = wx.Button(self.window, id=wx.ID_ADD)
104

    
105
                # Pretty Stuff
106
                hLine = wx.StaticLine(self.window, size=(600, 5))
107
                bottomHLine = wx.StaticLine(self.window, size=(600, 5))
108

    
109
                # Row 0
110
                sizer.Add(newScout)
111
                for i in range(7):
112
                        sizer.AddStretchSpacer(1)
113
                # Row 1
114
                sizer.AddMany([newScoutName, (newScoutInput, 0, wx.EXPAND), startXTitle, startX, startYTitle, startY, startThetaTitle, startTheta])
115
                # Row 2
116
                for i in range(7):
117
                        sizer.AddStretchSpacer(1)
118
                sizer.Add(addButton)
119
                # Row 3
120

    
121
                # Events
122
                addButton.Bind(wx.EVT_BUTTON, lambda event: self.NewScout(event, newScoutInput, startX, startY, startTheta))
123

    
124
                sizer.AddGrowableCol(idx=1)
125
                self.mainArea.Add(sizer, proportion=1, flag=wx.ALL|wx.EXPAND, border=10)
126
                self.window.SetSizer(self.mainArea)
127
                self.sizer = defaultdict()
128

    
129
        def NewScout(self, e, name, x, y, theta):
130
                if (name):
131
                        scoutCreated = self.AddScout(name.GetValue(), x.GetValue(), y.GetValue(), theta.GetValue())
132
                        if (scoutCreated):
133
                                wx.MessageBox("Scout '"+name.GetValue()+"' created successfully at ("+str(x.GetValue())+", "+str(y.GetValue())+") facing "+str(theta.GetValue())+u"\u00B0.", 'Scout Created', wx.OK | wx.ICON_INFORMATION)
134
                                name.SetValue("")
135
                                x.SetValue(0)
136
                                y.SetValue(0)
137
                                theta.SetValue(0)
138
                                return True
139
                wx.MessageBox('Error creating scout.  Please check that all information was entered properly and there are no duplicate scout names.', 'Info', wx.OK | wx.ICON_ERROR)
140
        
141
        def AddScoutBox(self, name):
142
                self.sizer[name] = wx.FlexGridSizer(rows=2, cols=4, hgap=5, vgap=5)
143
                # Labels
144
                scoutName = wx.StaticText(self.window, label="Scout: "+name)
145
                behaviorLabel = wx.StaticText(self.window, label="Behavior: ")
146
                currBehaviorLabel = wx.StaticText(self.window, label="  |  Current Behavior: Slacking off")
147

    
148
                # Inputs
149
                scoutChoices = wx.Choice(self.window, choices=GetBehaviors())
150
                pauseButton = wx.ToggleButton(self.window, label="Pause")
151
                runButton = wx.Button(self.window, label="Run")
152
                killButton = wx.Button(self.window, label="Kill")
153

    
154
                self.sizer[name].Add(scoutName)
155
                self.sizer[name].Add(currBehaviorLabel, wx.EXPAND|wx.ALIGN_RIGHT)
156
                self.sizer[name].AddStretchSpacer(1)
157
                self.sizer[name].Add(killButton, wx.ALIGN_RIGHT)
158

    
159
                self.sizer[name].Add(behaviorLabel)
160
                self.sizer[name].Add(scoutChoices, wx.EXPAND)
161
                self.sizer[name].Add(runButton)
162
                self.sizer[name].Add(pauseButton, wx.ALIGN_RIGHT)
163
                
164
                # Events
165
                pauseButton.Bind(wx.EVT_TOGGLEBUTTON, lambda event: PauseScout(name, currBehaviorLabel, pauseButton))
166
                runButton.Bind(wx.EVT_BUTTON, lambda event: SetBehavior(name, scoutChoices.GetStringSelection(), currBehaviorLabel))
167
                killButton.Bind(wx.EVT_BUTTON, lambda event: self.RemoveScout(name))
168

    
169
                self.mainArea.Add(self.sizer[name], proportion=1, flag=wx.ALL|wx.EXPAND, border=10)
170
                self.window.Layout()
171
                return True
172

    
173
        def AddScout(self, name, x, y, theta):
174
                if AddToSimulator(name, x, y, theta):
175
                        return self.AddScoutBox(name)
176
                wx.MessageBox("ROS call failed!", 'ROS Error', wx.OK | wx.ICON_ERROR)
177
                return False
178

    
179
        def RemoveScout(self, name):
180
                # I don't trust this bit. Remove() should work by itself, but it doesn't want to.
181
                # Hide()ing before Remove()ing does work for some reason though. It makes me think 
182
                # the sizer is still around somewhere, sneakily leaking memory.
183
                # Alas, such is life.
184
                print "Hidden scoutLayout for "+name+": "+str(self.mainArea.Hide(self.sizer[name], True))
185
                print "Removed scoutLayout for "+name+": "+str(self.mainArea.Remove(self.sizer[name]))+" (Maybe...)"
186
                del self.sizer[name]
187
                if KillScout(name):
188
                        self.window.Layout()
189
                        self.window.Refresh()
190
                        self.window.Update()
191
                return
192

    
193
if __name__ == '__main__':
194
        subprocess.Popen("rosrun scoutsim scoutsim_node race", shell=True)
195
        app = wx.App()
196
        GUI(None, title='Colony Scout Manager')
197
        app.MainLoop()