Statistics
| Branch: | Revision:

scoutos / scout / scoutsim / GUI.py @ 7db6cf9f

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