Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (7.46 KB)

1 5755691e Matt Bryant
#!/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 27f73b95 Matt Bryant
# Everyone global variables (PLEASE REFACTOR ME)
15 5755691e Matt Bryant
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 16d4e150 Priya
def SetBehavior(name, behaviorLabel, behaviorButton=False, behavior="Pause", selectedBehavior="Pause"):
38 5755691e Matt Bryant
        if behavior == "Pause":
39
                if name not in processes:
40
                        return False
41
                if not behaviorButton.GetValue():
42 16d4e150 Priya
                        processes[name] = subprocess.Popen("rosrun libscout libscout "+name+" "+str(GetBehaviors().index(selectedBehavior)+1)+"", shell=True)
43 5755691e Matt Bryant
                        print "Scout "+name+" was resumed."
44
                        behaviorButton.SetLabel("Pause")
45
                        behaviorLabel.SetLabel("  |  Current Behavior: "+scouts[name])
46
                else:
47 16d4e150 Priya
                        processes[name].kill()
48
                        processes[name].wait()
49 9a88eb2e Priya
                        subprocess.Popen("rosrun libscout libscout "+name+" 0", shell=True) 
50 5755691e Matt Bryant
                        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 16d4e150 Priya
                processes[name].wait()
57
                # Running a new behavior while paused does not do anything and
58
                # Gives lots of behaviors.
59 9a88eb2e Priya
        processes[name] = subprocess.Popen("rosrun libscout libscout "+name+" "+str(GetBehaviors().index(behavior)+1)+"", shell=True) 
60 27f73b95 Matt Bryant
        behaviorButton.SetValue(False)
61
        behaviorButton.SetLabel("Pause")
62 5755691e Matt Bryant
        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 16d4e150 Priya
                        # BUG: Pause-ing a scout and then killing it crashes the program.
72 5755691e Matt Bryant
                        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 16d4e150 Priya
def PauseScout(name, behaviorLabel, behaviorButton, selectedBehavior):
81
        return SetBehavior(name, behaviorLabel, behaviorButton, "Pause", selectedBehavior)
82 5755691e Matt Bryant
#
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 9a88eb2e Priya
                self.AddScoutBox("scout1")     
92 5755691e Matt Bryant
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 27f73b95 Matt Bryant
                startThetaTitle = wx.StaticText(self.window, label="Rad:")
105 5755691e Matt Bryant
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 27f73b95 Matt Bryant
                startTheta = wx.lib.intctrl.IntCtrl(self.window, min=0, max=100)
111 5755691e Matt Bryant
                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 16d4e150 Priya
                pauseButton.Bind(wx.EVT_TOGGLEBUTTON, lambda event: PauseScout(name, currBehaviorLabel, pauseButton, scoutChoices.GetStringSelection()))
174 27f73b95 Matt Bryant
                runButton.Bind(wx.EVT_BUTTON, lambda event: SetBehavior(name, currBehaviorLabel, pauseButton, scoutChoices.GetStringSelection()))
175 5755691e Matt Bryant
                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()