Statistics
| Branch: | Revision:

root / scout / scoutsim / GUI.py @ 6350051e

History | View | Annotate | Download (16.9 KB)

1 01f09975 Yuyang Guo
#!/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 6ee555a3 Priya
import sys
9 01f09975 Yuyang Guo
10
# GUI libs
11
import wx
12
import wx.lib.intctrl
13
import wx.lib.agw.floatspin
14
15
# not sure why these 2 are here
16
import string
17
import signal
18
19
# ros imports for spawning and killing scout
20
import rospy
21
import roslib
22 6350051e Alex
import os
23 01f09975 Yuyang Guo
roslib.load_manifest("scoutsim")
24
25
# scoutsim imports
26
from scoutsim.srv import *
27
28
from collections import defaultdict
29
30 9a4d8398 Yuyang Guo
31
class Behaviors(object):
32
    def __init__(self):
33
        print "Warning: You should not need to create an instance of Behaviors"
34
35
    # this takes advantage of the fact that our behavior list start at 1    
36
    behaviors = ["Pause", "CW Circle", "CCW Circle", "Odometry",
37
                          "Navigation Map", "Scheduler", "Warehouse",
38 6ee555a3 Priya
                          "Line Follow", "WL Test", "Maze Solve"]
39 9a4d8398 Yuyang Guo
    @classmethod
40 6350051e Alex
    def getListFromDir(self):
41
        behaviors = []
42
        for path, dirname, fnames in os.walk('../libscout/src'):
43
            if 'src/behaviors' in path or 'src/test_behaviors' in path:
44
                path_parts = path.split('/')
45
46
                for f in sorted(fnames):
47
                    # The pause_scout behavior needs to go first!
48
                    if f.endswith('.h') and 'pause_scout' in f:
49
                        behaviors.insert(0, f.split('.')[0])
50
                    # Everything else goes in alphabetical order
51
                    elif f.endswith('.h'):
52
                        behaviors.append(f.split('.')[0])
53
        return behaviors
54
55
    @classmethod
56 9a4d8398 Yuyang Guo
    def getNumber(self, behaviorName):
57 6350051e Alex
        behaviors = self.getListFromDir()
58
        if (behaviorName in behaviors):
59
            return behaviors.index(behaviorName)
60 9a4d8398 Yuyang Guo
    
61
    @classmethod
62
    def getName(self, index):
63 6350051e Alex
        behaviors = self.getListFromDir()
64 9a4d8398 Yuyang Guo
        if (0 <= index < len(self.Behavior)):
65 6350051e Alex
            return behaviors[index]
66 9a4d8398 Yuyang Guo
        else:
67
            return -1
68
    
69
    @classmethod
70
    def getBehaviors(self):
71
        # "Pause" is not considered a behavior in GUI
72 6350051e Alex
        behaviors = self.getListFromDir()
73
        return behaviors[1:]
74 9a4d8398 Yuyang Guo
75 01f09975 Yuyang Guo
# each scout is represented by this class
76
class Scout(object):
77
    numOfScouts = 0 # class variable keeping track of scouts
78
    
79
    def __init__(self, x=0, y=0, theta=0, name=u"",
80
                       Scouts=None, oninit = False):
81
        Scout.numOfScouts += 1 # reverted if not successful
82
        if len(name) == 0:
83
            # automatically give name according to numOfScouts
84
            name = self.autoNameGen(Scouts)
85
        # set vars
86
        self.name = name
87
        self.behavior = "chilling"
88
        self.process =  None# used to keep track of all processes
89
                        # related to this scout  
90 9a4d8398 Yuyang Guo
        self.paused = False
91 01f09975 Yuyang Guo
        self.valid = False
92
        # these vars are right now just the original position
93
        # but when ODOMETRY is done, we could actually use them
94
        self.x, self.y = x, y 
95
        self.theta = theta
96
        
97
        # spawn the scout if it is not scout1 (automatically spawned)
98
        if (not oninit): 
99
            if (self.SpawnInSimulator()):
100
                # spawned correctly
101
                self.valid = True
102
            else:
103
                Scout.numOfScouts -= 1;
104
        
105
106
    def autoNameGen(self, Scouts):
107
        i = 1
108
        while (True):
109
            if not (("scout%d"%i) in Scouts):
110
                return "scout%d"%i
111
            i+=1
112
113
114
        # update the classvariable
115
        
116
117
    def SpawnInSimulator(self):
118
        try:
119
            # ros spawn routine
120
            rospy.wait_for_service('/spawn');
121
            service = rospy.ServiceProxy('/spawn', Spawn);
122
            response = service(self.x, self.y, self.theta, self.name)
123
            return True
124
        except rospy.ServiceException as Error:
125
            return False
126 9a4d8398 Yuyang Guo
   
127
    def terminateOldBehavior(self):
128
        if self.process != None:
129
            # now terminate the process
130
            self.process.terminate() #TODO: this kills the process
131
                                     #      but maybe not the rosnode
132
                                     #      change to "rosnode kill" instead
133
            # make sure old behavior is terminated before we run the new one
134
            self.process.wait()
135 01f09975 Yuyang Guo
136 9a4d8398 Yuyang Guo
            # the following lines causes lag and does not solve real issues
137
            # first unregister the node
138 1e52c76b Yuyang Guo
            cmd = "rosnode kill /%sBehavior"%self.name
139
            temp = subprocess.Popen(cmd, shell=True)
140
            temp.wait() 
141 9a4d8398 Yuyang Guo
            # this node loses contact but still is not cleaned up in rosnode
142
            # however, this should not affect usage at all.
143 01f09975 Yuyang Guo
        
144 9a4d8398 Yuyang Guo
    def changeBehavior(self, behavior):
145
        self.behavior = behavior
146
        if (behavior == "pause"):
147
            self.paused = True
148
        else:
149
            self.paused = False
150
        self.terminateOldBehavior()
151 01f09975 Yuyang Guo
        # do rosprocess calls for new behavior
152 1e52c76b Yuyang Guo
        roscommand = ("rosrun libscout libscout %s %s"%
153 9873e344 Yuyang Guo
                            (Behaviors.getNumber(self.behavior), self.name))
154 1e52c76b Yuyang Guo
        self.process = subprocess.Popen(roscommand, shell=True)
155 01f09975 Yuyang Guo
156 ab8736ac Yuyang Guo
    def teleop(self):
157
        # teleop involved only one command \
158
        self.terminateOldBehavior()
159
        self.process = None
160
        cmd = "rosservice call /set_teleop %s"%self.name
161
        try:
162
            subprocess.Popen(shlex.split(cmd), shell=False)
163
        except:
164
            #not sure why this is happening....
165
            # seems to be a ros thing
166
            print "warning, socket error, ignored"
167 1e52c76b Yuyang Guo
    
168
    def sonar_viz(self, on_off):
169
        # teleop involved only one command \
170
        cmd = "rosservice call /set_sonar_viz %s %s"%(on_off, self.name)
171
        try:
172
            subprocess.Popen(shlex.split(cmd), shell=False)
173
        except:
174
            #not sure why this is happening....
175
            # seems to be a ros thing
176
            print "warning, socket error, ignored"
177 ab8736ac Yuyang Guo
178 01f09975 Yuyang Guo
    def killSelf(self):
179
        # terminate its current behavior
180 9a4d8398 Yuyang Guo
        self.terminateOldBehavior()
181
        
182 01f09975 Yuyang Guo
        # ros call to kill scout in simulator
183
        try:
184
            rospy.wait_for_service("/kill")
185
            service = rospy.ServiceProxy("/kill", Kill)
186
            response = service(self.name)
187
            Scout.numOfScouts -= 1
188
            self.valid = False
189
        except rospy.ServiceException, e:
190 ab8736ac Yuyang Guo
            print "warning: kill scout unsuccessful" 
191 01f09975 Yuyang Guo
            self.valid = True
192
        # remove from the class
193
194
195
# a wrapper for wx Frame
196
class Window(wx.App):
197
    def __init__(self, title=""):
198
        super(Window, self).__init__()
199
        self.initUIFrame()
200
201
    def initUIFrame(self):
202
        self.GUIFrame = GUI(None, "hallo!")
203
204
205
# actual GUI frame
206
class GUI(wx.Frame):
207
    def __init__(self, parent, title):
208
        super(GUI, self).__init__(parent, title=title,
209
            style=wx.DEFAULT_FRAME_STYLE ^ wx.RESIZE_BORDER, size=(600, 600))
210
        
211
        # open up scoutsim race
212 6ee555a3 Priya
        if len(sys.argv) > 1:
213
            map_name = sys.argv[1]
214
        else:
215
            map_name = "race"
216
        command = shlex.split("rosrun scoutsim scoutsim_node " + map_name)
217 01f09975 Yuyang Guo
        self.simWindowProcess = subprocess.Popen(command, shell=False)
218
        #rospy.wait_for_service('/spawn') #don't know why this is here
219
        
220
        # register call back for on close cleanup
221
        self.Bind(wx.EVT_CLOSE, self.onClose) 
222
223
224
        self.initData()
225
        self.initUI()
226
        self.Show(True)     
227
    
228
    # do clean up after user close the window
229
    def onClose(self, event):
230
        print "Cleaning up all Processes"
231
        
232
        # kill all the behavior processes
233
        for scout in self.scouts:
234
            process = self.scouts[scout].process
235
            if (process != None):
236
                process.terminate()
237
238
        # kill the scout sim window
239
        self.simWindowProcess.terminate()
240
        print "done"
241
        self.Destroy()
242
243
    def initData(self):
244
        self.scouts = {}
245 9a4d8398 Yuyang Guo
        #FIXME: these arguments are not right...fix them!!!
246 01f09975 Yuyang Guo
        self.scouts["scout1"] = Scout(x=0, y=0, theta=0, name="scout1",
247
                                      Scouts = self.scouts, oninit=True)
248
249
    # addButton callback
250
    def addScout(self, x_wx, y_wx, theta_wx, name_wx):
251
        # x, y, theta, name are given as wx Inputs
252
        x = x_wx.GetValue()
253
        y = y_wx.GetValue()
254
        theta = theta_wx.GetValue()
255
        name = name_wx.GetValue()
256
        newSc = Scout(x, y, theta, name, self.scouts)
257
        if (newSc.valid):
258
            # successful
259
            self.scouts[newSc.name] = newSc
260
            self.addScoutBox(newSc.name)
261
            # alert user spawn successful
262
            wx.MessageBox(u"Scout '%s' created successfully at (%d,%d,%d\u00B0)"
263
                            %(newSc.name, newSc.x, newSc.y, newSc.theta), 
264
                            "Scout Created", wx.OK | wx.ICON_INFORMATION)
265
266
        else:
267
            #failed to create scout
268
            wx.MessageBox("Scout Creation failed :( Check for duplicate name",
269
                            "Add Scout failed", wx.OK | wx.ICON_ERROR)
270
271
    def removeScout(self, name):
272
        sct = self.scouts[name]
273
        sct.killSelf()
274
        if (sct.valid == False):
275
            # successful, remove from the dictionary
276
            del self.scouts[name]
277 9a4d8398 Yuyang Guo
            # delete the refresh the display
278
            self.mainArea.Hide(self.sizer[name]) #required by wx before remove
279
            self.mainArea.Remove(self.sizer[name])
280 01f09975 Yuyang Guo
            del self.sizer[name]
281
            self.window.Layout()
282
            self.window.Refresh()
283
            self.window.Update()
284
        else:
285
            raise Exception
286 ab8736ac Yuyang Guo
    
287
    # runPressed
288 9a4d8398 Yuyang Guo
    # change UI display and invoke change Behavior in scout
289
    def changeBehavior(self, name, currBehaviorLabel, 
290
                        pauseButton, newBehavior):
291
        
292
        # to handle user pressing "Run" during pause
293
        if (newBehavior != "Pause" and self.scouts[name].paused == True):
294
            self.correctPauseButton(name, 
295
                                        pauseButton, pauseToResume=True)
296
        
297
        currBehaviorLabel.SetLabel(" | Current Behavior: %s"%newBehavior)
298
        scout = self.scouts[name]
299
        scout.changeBehavior(newBehavior)
300 ab8736ac Yuyang Guo
        self.correctTeleopButton(None)
301 9a4d8398 Yuyang Guo
302
303
    def buttonDown(button):
304
        return button.GetValue()
305
306
    def pauseResumeScout(self, name, pauseButton, currBehaviorLabel, dropMenu):
307
        if (pauseButton.GetValue() == True): # 
308
            # change behavior to Pause
309
            self.changeBehavior(name, currBehaviorLabel, pauseButton, "Pause")    
310
            # change button to "Resume" & label to "Pause"
311
            currBehaviorLabel.SetLabel("Pause")
312
            self.correctPauseButton(name, 
313
                                        pauseButton, pauseToResume=False)
314
        else:
315
            # resume
316
            # change behavior to whatever is in the drop down menu
317
            self.changeBehavior(name, currBehaviorLabel, pauseButton, 
318
                        dropMenu.GetStringSelection())
319
            self.correctPauseButton(name, 
320
                                        pauseButton, pauseToResume=True)
321 ab8736ac Yuyang Guo
322
    def teleop(self, name):
323
        self.correctTeleopButton(name)
324
        self.scouts[name].teleop()
325 9a4d8398 Yuyang Guo
326 1e52c76b Yuyang Guo
    def sonar_viz(self, name, sonar_vizButton):
327
        if (sonar_vizButton.GetValue() == True):
328
            # turn on the sonar viz
329
            self.scouts[name].sonar_viz("on")
330
        else:
331
            # turn off sonar viz
332
            self.scouts[name].sonar_viz("off")
333
334 01f09975 Yuyang Guo
    ############################ UI stuff here ###########################
335
    
336
    def initUI(self):
337 ab8736ac Yuyang Guo
        self.allTeleopButtons = {}
338 01f09975 Yuyang Guo
        self.initAddScoutArea()
339 ab8736ac Yuyang Guo
    
340
    def correctTeleopButton(self, teleopingName):
341
        for name in self.allTeleopButtons:
342
            if name == teleopingName:
343
                self.allTeleopButtons[name].SetValue(True)
344
            else:
345
                self.allTeleopButtons[name].SetValue(False)
346
                
347
348
    def correctPauseButton(self, name, pauseButton, pauseToResume):
349
        if (pauseToResume):
350
            print "correcting button!"
351
            self.scouts[name].paused = False
352
            pauseButton.SetValue(False) # unpress it
353
            pauseButton.SetLabel("Pause")
354
        else:
355
            self.scouts[name].paused = True
356
            pauseButton.SetValue(True) # press it
357
            pauseButton.SetLabel("Resume")
358 01f09975 Yuyang Guo
359
    # the labels and input boxes for adding a scout through GUI
360
    def initAddScoutArea(self):
361
        # all the layout stuff copied over: using grid layout
362
        # button callbacks are changed
363
        self.totalCols = 8
364
        self.window = wx.ScrolledWindow(self, style=wx.VSCROLL)
365
        self.mainArea = wx.GridSizer(cols=1)
366
        sizer = wx.FlexGridSizer(rows=4, cols=self.totalCols, hgap=5, vgap=5)
367
368
        # Labels
369
        blankText = wx.StaticText(self.window, label="")
370
        newScout = wx.StaticText(self.window, label="New Scout")
371
        newScoutName = wx.StaticText(self.window, label="Name:")
372
        startXTitle = wx.StaticText(self.window, label="X:")
373
        startYTitle = wx.StaticText(self.window, label="Y:")
374
        startThetaTitle = wx.StaticText(self.window, label="Rad:")
375
376
        # Inputs
377
        newScoutInput = wx.TextCtrl(self.window)
378
        startX = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
379
        startY = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
380
        startTheta = wx.lib.agw.floatspin.FloatSpin(self.window, min_val=0, digits=5)
381
        addButton = wx.Button(self.window, id=wx.ID_ADD)
382
383
        # Pretty Stuff
384
        hLine = wx.StaticLine(self.window, size=(600, 5))
385
        bottomHLine = wx.StaticLine(self.window, size=(600, 5))
386
        
387
        # Row 0: just the label add scout
388
        sizer.Add(newScout)
389
        for i in range(7):
390
            sizer.AddStretchSpacer(1)
391
        # Row 1: input(name), x coord, y coord, rad, 
392
        sizer.AddMany([newScoutName, (newScoutInput, 0, wx.EXPAND), startXTitle,
393
            startX, startYTitle, startY, startThetaTitle, startTheta])
394
        # Row 2: just the add button to the right
395
        for i in range(7):
396
            sizer.AddStretchSpacer(1)
397
        sizer.Add(addButton)
398
        # Row 3
399
        
400
        # Events
401
        addButton.Bind(wx.EVT_BUTTON, lambda event: self.addScout(
402
            startX, startY, startTheta, newScoutInput))
403
404
        sizer.AddGrowableCol(idx=1)
405
        self.mainArea.Add(sizer, proportion=1, flag=wx.ALL|wx.EXPAND, border=10)
406
        self.window.SetSizer(self.mainArea)
407
        self.sizer = defaultdict()
408
        self.window.Layout()
409
        
410
        # make the scout1's controller
411
        self.addScoutBox("scout1");
412 1e52c76b Yuyang Guo
    
413
    
414
415 01f09975 Yuyang Guo
    # copied over from old GUI
416
    def addScoutBox(self, name):
417
        self.sizer[name] = wx.FlexGridSizer(rows=2, cols=5, hgap=5, vgap=5)
418
        # Labels
419 9a4d8398 Yuyang Guo
        scoutName = wx.StaticText(self.window, label="Scout: %s"%name)
420 01f09975 Yuyang Guo
        behaviorLabel = wx.StaticText(self.window, label="Behavior: ")
421
        currBehaviorLabel = wx.StaticText(self.window,
422
            label="  |  Current Behavior: Slacking off")
423
424
        # Inputs
425
        # drop down menue
426 9a4d8398 Yuyang Guo
        scoutChoices = wx.Choice(self.window, 
427
                                    choices=Behaviors.getBehaviors())
428 01f09975 Yuyang Guo
        #   buttons
429
        pauseButton = wx.ToggleButton(self.window, label="Pause")
430
        runButton = wx.Button(self.window, label="Run")
431
        killButton = wx.Button(self.window, label="Kill")
432
        teleopButton = wx.ToggleButton(self.window, label="Teleop")
433 1e52c76b Yuyang Guo
        sonar_vizButton = wx.ToggleButton(self.window, label="sonar viz")
434 ab8736ac Yuyang Guo
        self.allTeleopButtons[name] = teleopButton
435 01f09975 Yuyang Guo
        # row 0
436
        self.sizer[name].Add(scoutName)
437
        self.sizer[name].Add(currBehaviorLabel, wx.EXPAND | wx.ALIGN_RIGHT)
438
        self.sizer[name].AddStretchSpacer(1)
439
        self.sizer[name].Add(killButton, wx.ALIGN_RIGHT)
440 1e52c76b Yuyang Guo
        self.sizer[name].Add(sonar_vizButton)
441 01f09975 Yuyang Guo
442
        # row 1
443
        self.sizer[name].Add(behaviorLabel)
444
        self.sizer[name].Add(scoutChoices, wx.EXPAND)
445
        self.sizer[name].Add(runButton)
446
        self.sizer[name].Add(pauseButton, wx.ALIGN_RIGHT)
447
        self.sizer[name].Add(teleopButton) 
448
        # Events
449
        killButton.Bind(wx.EVT_BUTTON, lambda event: self.removeScout(name))
450 9a4d8398 Yuyang Guo
        runButton.Bind(wx.EVT_BUTTON,
451
                lambda event: self.changeBehavior(name, currBehaviorLabel,
452
                    pauseButton, # needed to handle press "run" during pause
453
                    scoutChoices.GetStringSelection()))
454
        pauseButton.Bind(wx.EVT_TOGGLEBUTTON, 
455
            lambda event: self.pauseResumeScout(name, pauseButton,
456
                                        currBehaviorLabel, scoutChoices))
457 ab8736ac Yuyang Guo
        teleopButton.Bind(wx.EVT_TOGGLEBUTTON, 
458
                                lambda event: self.teleop(name))
459 1e52c76b Yuyang Guo
        sonar_vizButton.Bind(wx.EVT_TOGGLEBUTTON, 
460
                                lambda event: 
461
                                    self.sonar_viz(name, sonar_vizButton))
462 01f09975 Yuyang Guo
        self.mainArea.Add(self.sizer[name], proportion=1,
463
            flag=wx.ALL | wx.EXPAND, border=10)
464
        self.window.Layout()
465
        return True
466
467
468
469
if __name__ == '__main__':
470
    # open up GUI
471
    window = Window(title="Colony Scout Manager")
472
    window.MainLoop()