Statistics
| Branch: | Revision:

root / scout / scoutsim / BehaviorGUI.py @ 16d4e150

History | View | Annotate | Download (7.46 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 global variables (PLEASE REFACTOR ME)
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", selectedBehavior="Pause"):
38
        if behavior == "Pause":
39
                if name not in processes:
40
                        return False
41
                if not behaviorButton.GetValue():
42
                        processes[name] = subprocess.Popen("rosrun libscout libscout "+name+" "+str(GetBehaviors().index(selectedBehavior)+1)+"", shell=True)
43
                        print "Scout "+name+" was resumed."
44
                        behaviorButton.SetLabel("Pause")
45
                        behaviorLabel.SetLabel("  |  Current Behavior: "+scouts[name])
46
                else:
47
                        processes[name].kill()
48
                        processes[name].wait()
49
                        subprocess.Popen("rosrun libscout libscout "+name+" 0", shell=True) 
50
                        print "Scout "+name+" was paused."
51
                        behaviorButton.SetLabel("Resume")
52
                        behaviorLabel.SetLabel("  |  Current Behavior: Paused")
53
                return True
54
        if name in processes:
55
                processes[name].kill()
56
                processes[name].wait()
57
                # Running a new behavior while paused does not do anything and
58
                # Gives lots of behaviors.
59
        processes[name] = subprocess.Popen("rosrun libscout libscout "+name+" "+str(GetBehaviors().index(behavior)+1)+"", shell=True) 
60
        behaviorButton.SetValue(False)
61
        behaviorButton.SetLabel("Pause")
62
        print "Scout "+name+" was set to behavior: "+behavior
63
        behaviorLabel.SetLabel("  |  Current Behavior: "+behavior)
64
        scouts[name] = behavior
65

    
66
def KillScout(name):
67
        try:
68
                rospy.wait_for_service('/kill');
69
                service = rospy.ServiceProxy('/kill', Kill);
70
                if name in processes:
71
                        # BUG: Pause-ing a scout and then killing it crashes the program.
72
                        processes[name].kill() # This is perhaps not working
73
                resp = service(name)
74
                del scouts[name]
75
                del processes[name]
76
                return True
77
        except rospy.ServiceException, e:
78
                return False
79

    
80
def PauseScout(name, behaviorLabel, behaviorButton, selectedBehavior):
81
        return SetBehavior(name, behaviorLabel, behaviorButton, "Pause", selectedBehavior)
82
#
83
# End ROS functions
84
#
85

    
86
class GUI(wx.Frame):
87
        def __init__(self, parent, title):
88
                super(GUI, self).__init__(parent, title=title, style=wx.DEFAULT_FRAME_STYLE ^ wx.RESIZE_BORDER, size=(600, 600))
89
                self.InitUI()
90
                self.Show()     
91
                self.AddScoutBox("scout1")     
92

    
93
        def InitUI(self):
94
                self.window = wx.ScrolledWindow(self, style=wx.VSCROLL)
95
                self.mainArea = wx.GridSizer(cols=1)
96
                sizer = wx.FlexGridSizer(rows=4, cols=8, hgap=5, vgap=5)
97

    
98
                # Labels
99
                blankText = wx.StaticText(self.window, label="")
100
                newScout = wx.StaticText(self.window, label="New Scout")
101
                newScoutName = wx.StaticText(self.window, label="Name:")
102
                startXTitle = wx.StaticText(self.window, label="X:")
103
                startYTitle = wx.StaticText(self.window, label="Y:")
104
                startThetaTitle = wx.StaticText(self.window, label="Rad:")
105

    
106
                # Inputs
107
                newScoutInput = wx.TextCtrl(self.window)
108
                startX = wx.lib.intctrl.IntCtrl(self.window, min=0)
109
                startY = wx.lib.intctrl.IntCtrl(self.window, min=0)
110
                startTheta = wx.lib.intctrl.IntCtrl(self.window, min=0, max=100)
111
                addButton = wx.Button(self.window, id=wx.ID_ADD)
112

    
113
                # Pretty Stuff
114
                hLine = wx.StaticLine(self.window, size=(600, 5))
115
                bottomHLine = wx.StaticLine(self.window, size=(600, 5))
116

    
117
                # Row 0
118
                sizer.Add(newScout)
119
                for i in range(7):
120
                        sizer.AddStretchSpacer(1)
121
                # Row 1
122
                sizer.AddMany([newScoutName, (newScoutInput, 0, wx.EXPAND), startXTitle, startX, startYTitle, startY, startThetaTitle, startTheta])
123
                # Row 2
124
                for i in range(7):
125
                        sizer.AddStretchSpacer(1)
126
                sizer.Add(addButton)
127
                # Row 3
128

    
129
                # Events
130
                addButton.Bind(wx.EVT_BUTTON, lambda event: self.NewScout(event, newScoutInput, startX, startY, startTheta))
131

    
132
                sizer.AddGrowableCol(idx=1)
133
                self.mainArea.Add(sizer, proportion=1, flag=wx.ALL|wx.EXPAND, border=10)
134
                self.window.SetSizer(self.mainArea)
135
                self.sizer = defaultdict()
136

    
137
        def NewScout(self, e, name, x, y, theta):
138
                if (name):
139
                        scoutCreated = self.AddScout(name.GetValue(), x.GetValue(), y.GetValue(), theta.GetValue())
140
                        if (scoutCreated):
141
                                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)
142
                                name.SetValue("")
143
                                x.SetValue(0)
144
                                y.SetValue(0)
145
                                theta.SetValue(0)
146
                                return True
147
                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)
148
        
149
        def AddScoutBox(self, name):
150
                self.sizer[name] = wx.FlexGridSizer(rows=2, cols=4, hgap=5, vgap=5)
151
                # Labels
152
                scoutName = wx.StaticText(self.window, label="Scout: "+name)
153
                behaviorLabel = wx.StaticText(self.window, label="Behavior: ")
154
                currBehaviorLabel = wx.StaticText(self.window, label="  |  Current Behavior: Slacking off")
155

    
156
                # Inputs
157
                scoutChoices = wx.Choice(self.window, choices=GetBehaviors())
158
                pauseButton = wx.ToggleButton(self.window, label="Pause")
159
                runButton = wx.Button(self.window, label="Run")
160
                killButton = wx.Button(self.window, label="Kill")
161

    
162
                self.sizer[name].Add(scoutName)
163
                self.sizer[name].Add(currBehaviorLabel, wx.EXPAND|wx.ALIGN_RIGHT)
164
                self.sizer[name].AddStretchSpacer(1)
165
                self.sizer[name].Add(killButton, wx.ALIGN_RIGHT)
166

    
167
                self.sizer[name].Add(behaviorLabel)
168
                self.sizer[name].Add(scoutChoices, wx.EXPAND)
169
                self.sizer[name].Add(runButton)
170
                self.sizer[name].Add(pauseButton, wx.ALIGN_RIGHT)
171
                
172
                # Events
173
                pauseButton.Bind(wx.EVT_TOGGLEBUTTON, lambda event: PauseScout(name, currBehaviorLabel, pauseButton, scoutChoices.GetStringSelection()))
174
                runButton.Bind(wx.EVT_BUTTON, lambda event: SetBehavior(name, currBehaviorLabel, pauseButton, scoutChoices.GetStringSelection()))
175
                killButton.Bind(wx.EVT_BUTTON, lambda event: self.RemoveScout(name))
176

    
177
                self.mainArea.Add(self.sizer[name], proportion=1, flag=wx.ALL|wx.EXPAND, border=10)
178
                self.window.Layout()
179
                return True
180

    
181
        def AddScout(self, name, x, y, theta):
182
                if AddToSimulator(name, x, y, theta):
183
                        return self.AddScoutBox(name)
184
                wx.MessageBox("ROS call failed!", 'ROS Error', wx.OK | wx.ICON_ERROR)
185
                return False
186

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

    
201
if __name__ == '__main__':
202
        subprocess.Popen("rosrun scoutsim scoutsim_node race", shell=True)
203
        app = wx.App()
204
        GUI(None, title='Colony Scout Manager')
205
        app.MainLoop()