Statistics
| Branch: | Revision:

scoutos / scout / scoutsim / GUI.py @ ab8736ac

History | View | Annotate | Download (15.2 KB)

1
#!/usr/bin/python
2

    
3
# Behavior GUI
4

    
5
# shell subprocess libs
6
import subprocess
7
import shlex  #for converting string commands to tokens
8

    
9
# GUI libs
10
import wx
11
import wx.lib.intctrl
12
import wx.lib.agw.floatspin
13

    
14
# not sure why these 2 are here
15
import string
16
import signal
17

    
18
# ros imports for spawning and killing scout
19
import rospy
20
import roslib
21
roslib.load_manifest("scoutsim")
22

    
23
# scoutsim imports
24
from scoutsim.srv import *
25

    
26
from collections import defaultdict
27

    
28

    
29
class Behaviors(object):
30
    def __init__(self):
31
        print "Warning: You should not need to create an instance of Behaviors"
32

    
33
    # this takes advantage of the fact that our behavior list start at 1    
34
    behaviors = ["Pause", "CW Circle", "CCW Circle", "Odometry",
35
                          "Navigation Map", "Scheduler", "Warehouse",
36
                          "Line Follow", "WL Test"]
37
    @classmethod
38
    def getNumber(self, behaviorName):
39
        if (behaviorName in Behaviors.behaviors):
40
            return Behaviors.behaviors.index(behaviorName)
41
    
42
    @classmethod
43
    def getName(self, index):
44
        if (0 <= index < len(self.Behavior)):
45
            return Behaviors.behaviors[index]
46
        else:
47
            return -1
48
    
49
    @classmethod
50
    def getBehaviors(self):
51
        # "Pause" is not considered a behavior in GUI
52
        return Behaviors.behaviors[1:]
53

    
54
# each scout is represented by this class
55
class Scout(object):
56
    numOfScouts = 0 # class variable keeping track of scouts
57
    
58
    def __init__(self, x=0, y=0, theta=0, name=u"",
59
                       Scouts=None, oninit = False):
60
        Scout.numOfScouts += 1 # reverted if not successful
61
        if len(name) == 0:
62
            # automatically give name according to numOfScouts
63
            name = self.autoNameGen(Scouts)
64
        # set vars
65
        self.name = name
66
        self.behavior = "chilling"
67
        self.process =  None# used to keep track of all processes
68
                        # related to this scout  
69
        self.paused = False
70
        self.valid = False
71
        # these vars are right now just the original position
72
        # but when ODOMETRY is done, we could actually use them
73
        self.x, self.y = x, y 
74
        self.theta = theta
75
        
76
        # spawn the scout if it is not scout1 (automatically spawned)
77
        if (not oninit): 
78
            if (self.SpawnInSimulator()):
79
                # spawned correctly
80
                self.valid = True
81
            else:
82
                Scout.numOfScouts -= 1;
83
        
84

    
85
    def autoNameGen(self, Scouts):
86
        i = 1
87
        while (True):
88
            if not (("scout%d"%i) in Scouts):
89
                return "scout%d"%i
90
            i+=1
91

    
92

    
93
        # update the classvariable
94
        
95

    
96
    def SpawnInSimulator(self):
97
        try:
98
            # ros spawn routine
99
            rospy.wait_for_service('/spawn');
100
            service = rospy.ServiceProxy('/spawn', Spawn);
101
            response = service(self.x, self.y, self.theta, self.name)
102
            return True
103
        except rospy.ServiceException as Error:
104
            return False
105
   
106
    def terminateOldBehavior(self):
107
        if self.process != None:
108
            # now terminate the process
109
            self.process.terminate() #TODO: this kills the process
110
                                     #      but maybe not the rosnode
111
                                     #      change to "rosnode kill" instead
112
            # make sure old behavior is terminated before we run the new one
113
            self.process.wait()
114

    
115
            # the following lines causes lag and does not solve real issues
116
            # first unregister the node
117
            #cmd = "rosnode kill /%s_behavior"%self.name
118
            #temp = subprocess.Popen(cmd, shell=True)
119
            #temp.wait() 
120
            # this node loses contact but still is not cleaned up in rosnode
121
            # however, this should not affect usage at all.
122
        
123
    def changeBehavior(self, behavior):
124
        self.behavior = behavior
125
        if (behavior == "pause"):
126
            self.paused = True
127
        else:
128
            self.paused = False
129
        self.terminateOldBehavior()
130
        # do rosprocess calls for new behavior
131
        roscommand = shlex.split("rosrun libscout libscout %s %s"%
132
                            (self.name, Behaviors.getNumber(self.behavior)))
133
        self.process = subprocess.Popen(roscommand, shell=False)
134

    
135
    def teleop(self):
136
        # teleop involved only one command \
137
        self.terminateOldBehavior()
138
        self.process = None
139
        cmd = "rosservice call /set_teleop %s"%self.name
140
        try:
141
            subprocess.Popen(shlex.split(cmd), shell=False)
142
        except:
143
            #not sure why this is happening....
144
            # seems to be a ros thing
145
            print "warning, socket error, ignored"
146

    
147
    def killSelf(self):
148
        # terminate its current behavior
149
        self.terminateOldBehavior()
150
        
151
        # ros call to kill scout in simulator
152
        try:
153
            rospy.wait_for_service("/kill")
154
            service = rospy.ServiceProxy("/kill", Kill)
155
            response = service(self.name)
156
            Scout.numOfScouts -= 1
157
            self.valid = False
158
        except rospy.ServiceException, e:
159
            print "warning: kill scout unsuccessful" 
160
            self.valid = True
161
        # remove from the class
162

    
163

    
164
# a wrapper for wx Frame
165
class Window(wx.App):
166
    def __init__(self, title=""):
167
        super(Window, self).__init__()
168
        self.initUIFrame()
169

    
170
    def initUIFrame(self):
171
        self.GUIFrame = GUI(None, "hallo!")
172

    
173

    
174
# actual GUI frame
175
class GUI(wx.Frame):
176
    def __init__(self, parent, title):
177
        super(GUI, self).__init__(parent, title=title,
178
            style=wx.DEFAULT_FRAME_STYLE ^ wx.RESIZE_BORDER, size=(600, 600))
179
        
180
        # open up scoutsim race
181
        command = shlex.split("rosrun scoutsim scoutsim_node race")
182
        self.simWindowProcess = subprocess.Popen(command, shell=False)
183
        #rospy.wait_for_service('/spawn') #don't know why this is here
184
        
185
        # register call back for on close cleanup
186
        self.Bind(wx.EVT_CLOSE, self.onClose) 
187

    
188

    
189
        self.initData()
190
        self.initUI()
191
        self.Show(True)     
192
    
193
    # do clean up after user close the window
194
    def onClose(self, event):
195
        print "Cleaning up all Processes"
196
        
197
        # kill all the behavior processes
198
        for scout in self.scouts:
199
            process = self.scouts[scout].process
200
            if (process != None):
201
                process.terminate()
202

    
203
        # kill the scout sim window
204
        self.simWindowProcess.terminate()
205
        print "done"
206
        self.Destroy()
207

    
208
    def initData(self):
209
        self.scouts = {}
210
        #FIXME: these arguments are not right...fix them!!!
211
        self.scouts["scout1"] = Scout(x=0, y=0, theta=0, name="scout1",
212
                                      Scouts = self.scouts, oninit=True)
213

    
214
    # addButton callback
215
    def addScout(self, x_wx, y_wx, theta_wx, name_wx):
216
        # x, y, theta, name are given as wx Inputs
217
        x = x_wx.GetValue()
218
        y = y_wx.GetValue()
219
        theta = theta_wx.GetValue()
220
        name = name_wx.GetValue()
221
        newSc = Scout(x, y, theta, name, self.scouts)
222
        if (newSc.valid):
223
            # successful
224
            self.scouts[newSc.name] = newSc
225
            self.addScoutBox(newSc.name)
226
            # alert user spawn successful
227
            wx.MessageBox(u"Scout '%s' created successfully at (%d,%d,%d\u00B0)"
228
                            %(newSc.name, newSc.x, newSc.y, newSc.theta), 
229
                            "Scout Created", wx.OK | wx.ICON_INFORMATION)
230

    
231
        else:
232
            #failed to create scout
233
            wx.MessageBox("Scout Creation failed :( Check for duplicate name",
234
                            "Add Scout failed", wx.OK | wx.ICON_ERROR)
235

    
236
    def removeScout(self, name):
237
        sct = self.scouts[name]
238
        sct.killSelf()
239
        if (sct.valid == False):
240
            # successful, remove from the dictionary
241
            del self.scouts[name]
242
            # delete the refresh the display
243
            self.mainArea.Hide(self.sizer[name]) #required by wx before remove
244
            self.mainArea.Remove(self.sizer[name])
245
            del self.sizer[name]
246
            self.window.Layout()
247
            self.window.Refresh()
248
            self.window.Update()
249
        else:
250
            raise Exception
251
    
252
    # runPressed
253
    # change UI display and invoke change Behavior in scout
254
    def changeBehavior(self, name, currBehaviorLabel, 
255
                        pauseButton, newBehavior):
256
        
257
        # to handle user pressing "Run" during pause
258
        if (newBehavior != "Pause" and self.scouts[name].paused == True):
259
            self.correctPauseButton(name, 
260
                                        pauseButton, pauseToResume=True)
261
        
262
        currBehaviorLabel.SetLabel(" | Current Behavior: %s"%newBehavior)
263
        scout = self.scouts[name]
264
        scout.changeBehavior(newBehavior)
265
        self.correctTeleopButton(None)
266

    
267

    
268
    def buttonDown(button):
269
        return button.GetValue()
270

    
271
    def pauseResumeScout(self, name, pauseButton, currBehaviorLabel, dropMenu):
272
        if (pauseButton.GetValue() == True): # 
273
            # change behavior to Pause
274
            self.changeBehavior(name, currBehaviorLabel, pauseButton, "Pause")    
275
            # change button to "Resume" & label to "Pause"
276
            currBehaviorLabel.SetLabel("Pause")
277
            self.correctPauseButton(name, 
278
                                        pauseButton, pauseToResume=False)
279
        else:
280
            # resume
281
            # change behavior to whatever is in the drop down menu
282
            self.changeBehavior(name, currBehaviorLabel, pauseButton, 
283
                        dropMenu.GetStringSelection())
284
            self.correctPauseButton(name, 
285
                                        pauseButton, pauseToResume=True)
286

    
287
    def teleop(self, name):
288
        self.correctTeleopButton(name)
289
        self.scouts[name].teleop()
290

    
291
    ############################ UI stuff here ###########################
292
    
293
    def initUI(self):
294
        self.allTeleopButtons = {}
295
        self.initAddScoutArea()
296
    
297
    def correctTeleopButton(self, teleopingName):
298
        for name in self.allTeleopButtons:
299
            if name == teleopingName:
300
                self.allTeleopButtons[name].SetValue(True)
301
            else:
302
                self.allTeleopButtons[name].SetValue(False)
303
                
304

    
305
    def correctPauseButton(self, name, pauseButton, pauseToResume):
306
        if (pauseToResume):
307
            print "correcting button!"
308
            self.scouts[name].paused = False
309
            pauseButton.SetValue(False) # unpress it
310
            pauseButton.SetLabel("Pause")
311
        else:
312
            self.scouts[name].paused = True
313
            pauseButton.SetValue(True) # press it
314
            pauseButton.SetLabel("Resume")
315

    
316
    # the labels and input boxes for adding a scout through GUI
317
    def initAddScoutArea(self):
318
        # all the layout stuff copied over: using grid layout
319
        # button callbacks are changed
320
        self.totalCols = 8
321
        self.window = wx.ScrolledWindow(self, style=wx.VSCROLL)
322
        self.mainArea = wx.GridSizer(cols=1)
323
        sizer = wx.FlexGridSizer(rows=4, cols=self.totalCols, hgap=5, vgap=5)
324

    
325
        # Labels
326
        blankText = wx.StaticText(self.window, label="")
327
        newScout = wx.StaticText(self.window, label="New Scout")
328
        newScoutName = wx.StaticText(self.window, label="Name:")
329
        startXTitle = wx.StaticText(self.window, label="X:")
330
        startYTitle = wx.StaticText(self.window, label="Y:")
331
        startThetaTitle = wx.StaticText(self.window, label="Rad:")
332

    
333
        # Inputs
334
        newScoutInput = wx.TextCtrl(self.window)
335
        startX = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
336
        startY = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
337
        startTheta = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
338
        addButton = wx.Button(self.window, id=wx.ID_ADD)
339

    
340
        # Pretty Stuff
341
        hLine = wx.StaticLine(self.window, size=(600, 5))
342
        bottomHLine = wx.StaticLine(self.window, size=(600, 5))
343
        
344
        # Row 0: just the label add scout
345
        sizer.Add(newScout)
346
        for i in range(7):
347
            sizer.AddStretchSpacer(1)
348
        # Row 1: input(name), x coord, y coord, rad, 
349
        sizer.AddMany([newScoutName, (newScoutInput, 0, wx.EXPAND), startXTitle,
350
            startX, startYTitle, startY, startThetaTitle, startTheta])
351
        # Row 2: just the add button to the right
352
        for i in range(7):
353
            sizer.AddStretchSpacer(1)
354
        sizer.Add(addButton)
355
        # Row 3
356
        
357
        # Events
358
        addButton.Bind(wx.EVT_BUTTON, lambda event: self.addScout(
359
            startX, startY, startTheta, newScoutInput))
360

    
361
        sizer.AddGrowableCol(idx=1)
362
        self.mainArea.Add(sizer, proportion=1, flag=wx.ALL|wx.EXPAND, border=10)
363
        self.window.SetSizer(self.mainArea)
364
        self.sizer = defaultdict()
365
        self.window.Layout()
366
        
367
        # make the scout1's controller
368
        self.addScoutBox("scout1");
369
        
370
    # copied over from old GUI
371
    def addScoutBox(self, name):
372
        self.sizer[name] = wx.FlexGridSizer(rows=2, cols=5, hgap=5, vgap=5)
373
        # Labels
374
        scoutName = wx.StaticText(self.window, label="Scout: %s"%name)
375
        behaviorLabel = wx.StaticText(self.window, label="Behavior: ")
376
        currBehaviorLabel = wx.StaticText(self.window,
377
            label="  |  Current Behavior: Slacking off")
378

    
379
        # Inputs
380
        # drop down menue
381
        scoutChoices = wx.Choice(self.window, 
382
                                    choices=Behaviors.getBehaviors())
383
        #   buttons
384
        pauseButton = wx.ToggleButton(self.window, label="Pause")
385
        runButton = wx.Button(self.window, label="Run")
386
        killButton = wx.Button(self.window, label="Kill")
387
        teleopButton = wx.ToggleButton(self.window, label="Teleop")
388
        self.allTeleopButtons[name] = teleopButton
389
        # row 0
390
        self.sizer[name].Add(scoutName)
391
        self.sizer[name].Add(currBehaviorLabel, wx.EXPAND | wx.ALIGN_RIGHT)
392
        self.sizer[name].AddStretchSpacer(1)
393
        self.sizer[name].AddStretchSpacer(1)
394
        self.sizer[name].Add(killButton, wx.ALIGN_RIGHT)
395

    
396
        # row 1
397
        self.sizer[name].Add(behaviorLabel)
398
        self.sizer[name].Add(scoutChoices, wx.EXPAND)
399
        self.sizer[name].Add(runButton)
400
        self.sizer[name].Add(pauseButton, wx.ALIGN_RIGHT)
401
        self.sizer[name].Add(teleopButton) 
402
        # Events
403
        killButton.Bind(wx.EVT_BUTTON, lambda event: self.removeScout(name))
404
        runButton.Bind(wx.EVT_BUTTON,
405
                lambda event: self.changeBehavior(name, currBehaviorLabel,
406
                    pauseButton, # needed to handle press "run" during pause
407
                    scoutChoices.GetStringSelection()))
408
        pauseButton.Bind(wx.EVT_TOGGLEBUTTON, 
409
            lambda event: self.pauseResumeScout(name, pauseButton,
410
                                        currBehaviorLabel, scoutChoices))
411
        teleopButton.Bind(wx.EVT_TOGGLEBUTTON, 
412
                                lambda event: self.teleop(name))
413

    
414
        self.mainArea.Add(self.sizer[name], proportion=1,
415
            flag=wx.ALL | wx.EXPAND, border=10)
416
        self.window.Layout()
417
        return True
418

    
419

    
420

    
421
if __name__ == '__main__':
422
    # open up GUI
423
    window = Window(title="Colony Scout Manager")
424
    window.MainLoop()